447 lines
14 KiB
C++
447 lines
14 KiB
C++
/* -----------------------------------------------------------------------------
|
|
GSFramework
|
|
Copyright 2001-2013 Emmanuel Julien. All Rights Reserved.
|
|
----------------------------------------------------------------------------- */
|
|
|
|
|
|
#include <cmath>
|
|
#include <cfloat>
|
|
#include <cstring>
|
|
#include "core/path_kdtree.h"
|
|
#include "core/geometry.h"
|
|
#include "log/log.h"
|
|
#include "core/renderer_toolbox.h"
|
|
|
|
using namespace GS;
|
|
using namespace GS::Core;
|
|
using GS::Render::Renderer;
|
|
|
|
|
|
PathKdtree::KDTreeNode::KDTreeNode():m_KDTREE_NODE_ID_SEGMENT(NULL){ memset(m_KDTREE_NODE_ID_ROPE, -1, sizeof(int)*6);};
|
|
|
|
#define KDTREE_MAX_DEPTH 20
|
|
#define KDTREE_MAX_POLY_PER_NODE 5
|
|
|
|
void PathKdtree::DrawKdtreeNode(Renderer &render, int _CurrentNode, Matrix4& m)
|
|
{
|
|
MinMax min_max;
|
|
|
|
min_max.mn.x = m_NodeTree[_CurrentNode].m_KDTREE_NODE_AABB[KDTREE_SIDE_LEFT];
|
|
min_max.mn.y = m_NodeTree[_CurrentNode].m_KDTREE_NODE_AABB[KDTREE_SIDE_BOTTOM];
|
|
min_max.mn.z = m_NodeTree[_CurrentNode].m_KDTREE_NODE_AABB[KDTREE_SIDE_BACK];
|
|
min_max.mx.x = m_NodeTree[_CurrentNode].m_KDTREE_NODE_AABB[KDTREE_SIDE_RIGHT];
|
|
min_max.mx.y = m_NodeTree[_CurrentNode].m_KDTREE_NODE_AABB[KDTREE_SIDE_TOP];
|
|
min_max.mx.z = m_NodeTree[_CurrentNode].m_KDTREE_NODE_AABB[KDTREE_SIDE_FRONT];
|
|
|
|
min_max.mn = min_max.mn*m;
|
|
min_max.mx = min_max.mx*m;
|
|
|
|
RendererToolbox::DrawAABB(render, min_max);
|
|
|
|
if(!m_NodeTree[_CurrentNode].m_KDTREE_NODE_IS_LEAF)
|
|
{
|
|
DrawKdtreeNode(render, m_NodeTree[_CurrentNode].m_KDTREE_NODE_ID_CHILD_RIGHT, m);
|
|
DrawKdtreeNode(render, m_NodeTree[_CurrentNode].m_KDTREE_NODE_ID_CHILD_LEFT, m);
|
|
}
|
|
}
|
|
|
|
void PathKdtree::draw_scene_debug(Renderer &render, Matrix4& m)
|
|
{
|
|
DrawKdtreeNode(render, 0, m);
|
|
}
|
|
|
|
void PathKdtree::NearestQuadtreeTreeNode(Vector4 p, SharedArrayList<nMSegment*> &list_segment)
|
|
//------------------------------------------------------------------------------------------------------------------------
|
|
{
|
|
// go inside the quadtree
|
|
ArrayList<int> list_id_segment;
|
|
|
|
if(segment_list.GetCount() <= 0)
|
|
return;
|
|
|
|
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:
|
|
{
|
|
float l_X = p.x ;
|
|
if(l_X == m_NodeTree[l_CurrentNode].m_KDTREE_NODE_VALUE_SPLIT)
|
|
l_X += 0.001f;
|
|
|
|
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 = p.y ;
|
|
if(l_Y == m_NodeTree[l_CurrentNode].m_KDTREE_NODE_VALUE_SPLIT)
|
|
l_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 = p.z;
|
|
if(l_Z == m_NodeTree[l_CurrentNode].m_KDTREE_NODE_VALUE_SPLIT)
|
|
l_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;
|
|
}
|
|
}
|
|
|
|
int* l_TempPntIdSegment = m_NodeTree[l_CurrentNode].m_KDTREE_NODE_ID_SEGMENT;
|
|
int l_CountSegment = m_NodeTree[l_CurrentNode].m_KDTREE_NODE_COUNT_SEGMENT;
|
|
|
|
for(int i=0; i< l_CountSegment; ++i)
|
|
{
|
|
list_segment.Add(segment_list[*l_TempPntIdSegment]);
|
|
++l_TempPntIdSegment;
|
|
}
|
|
}
|
|
|
|
//-------------------------------------------------------------------
|
|
void PathKdtree::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_SEGMENT)
|
|
{
|
|
l_tempCopy[i].m_KDTREE_NODE_ID_SEGMENT = new int[m_NodeTree[i].m_KDTREE_NODE_COUNT_SEGMENT];
|
|
memcpy( l_tempCopy[i].m_KDTREE_NODE_ID_SEGMENT, m_NodeTree[i].m_KDTREE_NODE_ID_SEGMENT, sizeof(int)*m_NodeTree[i].m_KDTREE_NODE_COUNT_SEGMENT);
|
|
}
|
|
}
|
|
|
|
m_SizeTree += _IncreaseSize;
|
|
|
|
delete []m_NodeTree;
|
|
m_NodeTree = l_tempCopy;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------------------------------------------------------------------------------------------
|
|
void PathKdtree::CreateNodeKdtree(int &_CurrentNode, int *_IdSegment, int _CountSegment, int _CurrentDepth, bool _ForceCreateLeaf )
|
|
//----------------------------------------------------------------------------------------------------------------------------------------------------------------
|
|
{
|
|
// 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(_ForceCreateLeaf || _CountSegment < 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 + _CountSegment))
|
|
{
|
|
IncreaseSizeNodeKdtreeBuffer(10000 + _CountSegment);
|
|
}
|
|
|
|
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_SEGMENT = _CountSegment;
|
|
|
|
m_NodeTree[_CurrentNode].m_KDTREE_NODE_ID_SEGMENT = new int[_CountSegment];
|
|
memcpy( m_NodeTree[_CurrentNode].m_KDTREE_NODE_ID_SEGMENT, _IdSegment, sizeof(int)*_CountSegment);
|
|
|
|
// set the new id to set back
|
|
++_CurrentNode;
|
|
}
|
|
else
|
|
{
|
|
m_NodeTree[_CurrentNode].m_KDTREE_NODE_IS_LEAF = false;
|
|
|
|
//find the correct split axe X or Z
|
|
float l_TempValueSplit = 0.0f;
|
|
|
|
int l_CountSegmentOnX = 0;
|
|
int l_CountSegmentOnZ = 0;
|
|
|
|
// get the middle of the aabb
|
|
float l_X_MiddleAABB = (l_TempAABB[KDTREE_X_AXIS*2] + l_TempAABB[KDTREE_X_AXIS*2+1])*0.5f;
|
|
float l_Z_MiddleAABB = (l_TempAABB[KDTREE_Z_AXIS*2] + l_TempAABB[KDTREE_Z_AXIS*2+1])*0.5f;
|
|
|
|
for(int i=0; i<_CountSegment; ++i)
|
|
{
|
|
if(segment_list[_IdSegment[i]]->bounding_box.GetCenter().x > l_X_MiddleAABB)
|
|
++l_CountSegmentOnX;
|
|
else
|
|
--l_CountSegmentOnX;
|
|
|
|
if(segment_list[_IdSegment[i]]->bounding_box.GetCenter().z > l_Z_MiddleAABB)
|
|
++l_CountSegmentOnZ;
|
|
else
|
|
--l_CountSegmentOnZ;
|
|
}
|
|
|
|
//set new axis
|
|
int l_NewAxis;
|
|
if(Types::Abs(l_CountSegmentOnX) < Types::Abs(l_CountSegmentOnZ))
|
|
l_NewAxis = KDTREE_X_AXIS;
|
|
else
|
|
l_NewAxis = KDTREE_Z_AXIS;
|
|
|
|
// problem , we need absolutly leaf with some path inside, bad split function, so patch it
|
|
if((l_NewAxis == KDTREE_X_AXIS && _CountSegment == Types::Abs(l_CountSegmentOnX)) || (l_NewAxis == KDTREE_Z_AXIS && _CountSegment == Types::Abs(l_CountSegmentOnZ)))
|
|
{
|
|
CreateNodeKdtree(_CurrentNode, _IdSegment, _CountSegment, _CurrentDepth, true );
|
|
return;
|
|
}
|
|
|
|
// axis check with the length and width
|
|
float diff_axis_aabb = (l_TempAABB[KDTREE_X_AXIS*2+1] - l_TempAABB[KDTREE_X_AXIS*2]) / (l_TempAABB[KDTREE_Z_AXIS*2+1] - l_TempAABB[KDTREE_Z_AXIS*2]);
|
|
if(diff_axis_aabb > 1.5)
|
|
l_NewAxis = KDTREE_X_AXIS;
|
|
if(diff_axis_aabb < 0.66)
|
|
l_NewAxis = KDTREE_Z_AXIS;
|
|
|
|
m_NodeTree[_CurrentNode].m_KDTREE_NODE_TYPE_SPLIT = l_NewAxis;
|
|
|
|
// get the middle of the aabb
|
|
float l_MiddleAABB = (l_TempAABB[l_NewAxis*2] + l_TempAABB[l_NewAxis*2+1])*0.5f;
|
|
l_TempValueSplit = l_MiddleAABB;
|
|
|
|
m_NodeTree[_CurrentNode].m_KDTREE_NODE_VALUE_SPLIT = l_TempValueSplit;
|
|
|
|
// create the 2 childs
|
|
|
|
// 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;
|
|
|
|
// 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_NewAxis*2+1] = l_TempValueSplit;
|
|
|
|
int *l_IdLeftSegmentList = new int [_CountSegment];
|
|
int l_IdLeftCount = 0;
|
|
|
|
for(int i=0; i<_CountSegment; ++i)
|
|
{
|
|
bool l_Include = false;
|
|
switch(l_NewAxis)
|
|
{
|
|
case KDTREE_X_AXIS:
|
|
if(segment_list[_IdSegment[i]]->a.x <= l_TempValueSplit ||
|
|
segment_list[_IdSegment[i]]->b.x <= l_TempValueSplit)
|
|
l_Include = true;
|
|
break;
|
|
case KDTREE_Z_AXIS:
|
|
if(segment_list[_IdSegment[i]]->a.z <= l_TempValueSplit ||
|
|
segment_list[_IdSegment[i]]->b.z <= l_TempValueSplit)
|
|
l_Include = true;
|
|
break;
|
|
}
|
|
|
|
if(l_Include)
|
|
{
|
|
l_IdLeftSegmentList[l_IdLeftCount] = _IdSegment[i];
|
|
|
|
++l_IdLeftCount;
|
|
}
|
|
}
|
|
|
|
// 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];
|
|
memcpy(l_tempCopy, l_IdLeftSegmentList, sizeof(int)*l_IdLeftCount);
|
|
|
|
delete []l_IdLeftSegmentList;
|
|
l_IdLeftSegmentList = l_tempCopy;
|
|
}
|
|
|
|
CreateNodeKdtree(l_NewIdChildLeft, l_IdLeftSegmentList, l_IdLeftCount, _CurrentDepth+1);
|
|
|
|
l_IdInBigArray = l_NewIdChildLeft;
|
|
|
|
delete []l_IdLeftSegmentList;
|
|
}
|
|
|
|
// 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;
|
|
|
|
// 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_NewAxis*2] = l_TempValueSplit;
|
|
|
|
int *l_IdRightSegmentList = new int [_CountSegment];
|
|
int l_IdRightCount = 0;
|
|
|
|
for(int i=0; i<_CountSegment; ++i)
|
|
{
|
|
bool l_Include = false;
|
|
switch(l_NewAxis)
|
|
{
|
|
case KDTREE_X_AXIS:
|
|
if(segment_list[_IdSegment[i]]->a.x >= l_TempValueSplit ||
|
|
segment_list[_IdSegment[i]]->b.x >= l_TempValueSplit)
|
|
l_Include = true;
|
|
break;
|
|
case KDTREE_Z_AXIS:
|
|
if(segment_list[_IdSegment[i]]->a.z >= l_TempValueSplit ||
|
|
segment_list[_IdSegment[i]]->b.z >= l_TempValueSplit)
|
|
l_Include = true;
|
|
break;
|
|
}
|
|
|
|
if(l_Include)
|
|
{
|
|
l_IdRightSegmentList[l_IdRightCount] = _IdSegment[i];
|
|
|
|
++l_IdRightCount;
|
|
}
|
|
}
|
|
|
|
// 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];
|
|
memcpy(l_tempCopy, l_IdRightSegmentList, sizeof(int)*l_IdRightCount);
|
|
|
|
delete []l_IdRightSegmentList;
|
|
l_IdRightSegmentList = l_tempCopy;
|
|
}
|
|
|
|
CreateNodeKdtree(l_NewIdChildRight, l_IdRightSegmentList, l_IdRightCount, _CurrentDepth+1);
|
|
|
|
//set the new id for the next node in the stack
|
|
_CurrentNode = l_NewIdChildRight;
|
|
|
|
delete []l_IdRightSegmentList;
|
|
}
|
|
}
|
|
}
|
|
|
|
//--------------------------------------------------------------------------------
|
|
void PathKdtree::BuildQuadtree()
|
|
//--------------------------------------------------------------------------------
|
|
{
|
|
if(segment_list.GetCount() <= 0)
|
|
return;
|
|
|
|
//very not powerful kdtree construction
|
|
|
|
m_SizeTree = segment_list.GetCount()*4;
|
|
m_NodeTree = new KDTreeNode[m_SizeTree];
|
|
|
|
int m_CurrentNode = 0;
|
|
|
|
m_NodeTree[m_CurrentNode].m_KDTREE_NODE_TYPE_SPLIT = KDTREE_X_AXIS;
|
|
|
|
// find the big bounding box
|
|
MinMax max_min_max = segment_list[0]->GetBoundingBox();
|
|
ArrayListForeachPtr(nMSegment*, segment, segment_list)
|
|
{
|
|
max_min_max.Grow(segment->GetBoundingBox());
|
|
}
|
|
max_min_max.mn.y -= 100.0f;
|
|
max_min_max.mx.y += 100.0f;
|
|
|
|
float * l_TempAABB = m_NodeTree[m_CurrentNode].m_KDTREE_NODE_AABB;
|
|
|
|
l_TempAABB[KDTREE_SIDE_LEFT] = max_min_max.mn.x;
|
|
l_TempAABB[KDTREE_SIDE_BOTTOM] = max_min_max.mn.y;
|
|
l_TempAABB[KDTREE_SIDE_BACK] = max_min_max.mn.z;
|
|
l_TempAABB[KDTREE_SIDE_RIGHT] = max_min_max.mx.x;
|
|
l_TempAABB[KDTREE_SIDE_TOP] = max_min_max.mx.y;
|
|
l_TempAABB[KDTREE_SIDE_FRONT] = max_min_max.mx.z;
|
|
|
|
|
|
// to build the kdtree: id of the poly
|
|
int* l_IdSegment = new int[segment_list.GetCount()];
|
|
for(uint i=0; i<segment_list.GetCount(); ++i)
|
|
l_IdSegment[i] = i;
|
|
|
|
CreateNodeKdtree(m_CurrentNode, l_IdSegment, segment_list.GetCount(), 0);
|
|
|
|
_safe_delete_array(l_IdSegment);
|
|
}
|
|
|
|
//-------------------------------------------------------------------
|
|
bool PathKdtree::AddSegment(nMSegment* segment)
|
|
//-------------------------------------------------------------------
|
|
{
|
|
segment_list.Add(segment);
|
|
|
|
return true;
|
|
}
|
|
|
|
//------------------------------------------------------------------------------------------
|
|
bool PathKdtree::AddSegment(SharedArrayList<nMSegment*> _segment_list)
|
|
//------------------------------------------------------------------------------------------
|
|
{
|
|
ArrayListForeachPtr(nMSegment*, segment, _segment_list)
|
|
segment_list.Add(segment);
|
|
|
|
return true;
|
|
}
|
|
//---------------------------------------------------
|
|
bool PathKdtree::InsideKdTree(const Vector4 &s)
|
|
//---------------------------------------------------
|
|
{
|
|
if(m_NodeTree[0].m_KDTREE_NODE_AABB[0] <= s.x && s.x <= m_NodeTree[0].m_KDTREE_NODE_AABB[1] &&
|
|
m_NodeTree[0].m_KDTREE_NODE_AABB[2] <= s.y && s.y <= m_NodeTree[0].m_KDTREE_NODE_AABB[3] &&
|
|
m_NodeTree[0].m_KDTREE_NODE_AABB[4] <= s.z && s.z <= m_NodeTree[0].m_KDTREE_NODE_AABB[5] )
|
|
return true;
|
|
else
|
|
return false;
|
|
}
|
|
|
|
//-----------------------------------------
|
|
void PathKdtree::Free()
|
|
//-----------------------------------------
|
|
{
|
|
_safe_delete_array(m_NodeTree);
|
|
m_count_bih = 0;
|
|
}
|
|
|
|
PathKdtree::PathKdtree()
|
|
{
|
|
m_NodeTree = NULL;
|
|
m_count_bih = 0;
|
|
}
|