37 template<
unsigned char K>
59 void process( uint32_t
id,
float distSqrd,
float &maxDistSqrd ) {}
62 template <
typename NodeData,
unsigned char K=3,
class LookupProc = NullLookupProc>
class KdTree {
67 template<
typename NodeDataVector>
70 template<
typename NodeDataVector>
76 void recursiveBuild( uint32_t nodeNum, uint32_t
start, uint32_t
end, std::vector<NodeDataIndex> &buildNodes );
77 void lookup(
const NodeData &
p,
const LookupProc &process,
float maxDist )
const;
78 void findNearest(
float p[K],
float result[K], uint32_t *resultIndex )
const;
82 void privateLookup(uint32_t nodeNum,
float p[K],
const LookupProc &process,
float &maxDistSquared)
const;
83 void privateFindNearest( uint32_t nodeNum,
float p[K],
float &maxDistSquared,
float result[K], uint32_t *resultIndex )
const;
87 uint32_t nNodes, nextFreeNode;
92 template<
typename NDV>
95 static uint32_t
getSize(
const NDV &ndv ) {
96 return static_cast<uint32_t
>( ndv.size() );
100 template<
typename NodeData>
104 if( axis == 0 )
return data.x;
105 else if( axis == 1 )
return data.y;
106 else return (
float)data.z;
108 static float getAxis0(
const NodeData &
data ) {
return static_cast<float>( data.x ); }
109 static float getAxis1(
const NodeData &
data ) {
return static_cast<float>( data.y ); }
110 static float getAxis2(
const NodeData &
data ) {
return static_cast<float>( data.z ); }
112 float result = ( data.x - k[0] ) * ( data.x - k[0] );
113 result += ( data.y - k[1] ) * ( data.y - k[1] );
114 result += ( data.z - k[2] ) * ( data.z - k[2] );
123 if( axis == 0 )
return data.
x;
129 float result = ( data.
x - k[0] ) * ( data.
x - k[0] );
130 result += ( data.
y - k[1] ) * ( data.
y - k[1] );
138 bool operator()(
const std::pair<const NodeData*,uint32_t> &d1,
139 const std::pair<const NodeData*,uint32_t> &d2)
const {
146 template<
typename NodeData,
unsigned char K,
typename LookupProc>
147 template<
typename NodeDataVector>
153 template<
typename NodeData,
unsigned char K,
typename LookupProc>
154 template<
typename NodeDataVector>
161 std::vector<NodeDataIndex> buildNodes;
162 buildNodes.reserve( nNodes );
163 for( uint32_t i = 0; i < nNodes; ++i )
164 buildNodes.push_back( std::make_pair( &d[i], i ) );
166 recursiveBuild( 0, 0, nNodes, buildNodes );
169 template<
typename NodeData,
unsigned char K,
typename LookupProc>
173 if( start + 1 == end) {
174 nodes[nodeNum].initLeaf();
175 mNodeData[nodeNum] = buildNodes[
start];
180 float boundMin[K], boundMax[K];
181 for(
unsigned char k = 0; k < K; ++k ) {
182 boundMin[k] = FLT_MAX;
183 boundMax[k] = FLT_MIN;
186 for( uint32_t i = start; i <
end; ++i ) {
187 for( uint8_t axis = 0; axis < K; axis++ ) {
194 float maxExtent = boundMax[0] - boundMin[0];
195 for(
unsigned char k = 1; k < K; ++k ) {
196 if( boundMax[k] - boundMin[k] > maxExtent ) {
198 maxExtent = boundMax[k] - boundMin[k];
201 uint32_t splitPos = ( start +
end ) / 2;
202 std::nth_element( &buildNodes[start], &buildNodes[splitPos], &buildNodes[end-1] + 1,
CompareNode<NodeData>(splitAxis) );
205 mNodeData[nodeNum] = buildNodes[splitPos];
206 if( start < splitPos ) {
207 nodes[nodeNum].hasLeftChild = 1;
208 uint32_t childNum = nextFreeNode++;
209 recursiveBuild( childNum, start, splitPos, buildNodes );
211 if( splitPos + 1 < end ) {
212 nodes[nodeNum].rightChild = nextFreeNode++;
213 recursiveBuild( nodes[nodeNum].rightChild, splitPos + 1, end, buildNodes );
217 template<
typename NodeData,
unsigned char K,
typename LookupProc>
220 float maxDistSqrd = maxDist * maxDist;
222 for(
unsigned char k = 0; k < K; ++k )
225 privateLookup( 0, pt, proc, maxDistSqrd );
228 template<
typename NodeData,
unsigned char K,
typename LookupProc>
238 privateLookup( nodeNum + 1,
p, process, maxDistSquared );
239 if( ( dist2 < maxDistSquared ) && ( node->
rightChild < nNodes ) )
240 privateLookup( node->
rightChild,
p, process, maxDistSquared );
244 privateLookup( node->
rightChild,
p, process, maxDistSquared );
246 privateLookup( nodeNum + 1,
p, process, maxDistSquared );
250 float distSqr = 0.0f;
251 for(
unsigned char k = 0; k < K; ++k ) {
256 if( distSqr < maxDistSquared )
257 process.process( mNodeData[nodeNum].second, distSqr, maxDistSquared );
261 template<
typename NodeData,
unsigned char K,
typename LookupProc>
264 float maxDist = FLT_MAX;
266 privateFindNearest( 0,
p, maxDist, result, resultIndex );
269 template<
typename NodeData,
unsigned char K,
typename LookupProc>
279 privateFindNearest( nodeNum+1,
p, maxDistSquared, result, resultIndex );
280 if( ( dist2 < maxDistSquared ) && ( node->
rightChild < nNodes) )
281 privateFindNearest( node->
rightChild,
p, maxDistSquared, result, resultIndex );
287 maxDistSquared, result, resultIndex );
288 if( dist2 < maxDistSquared && node->hasLeftChild)
289 privateFindNearest(nodeNum+1,
291 maxDistSquared, result, resultIndex );
296 if( distSqr < maxDistSquared ) {
297 maxDistSquared = distSqr;
298 for(
unsigned char k = 0; k < K; ++k )
299 result[k] = NodeDataTraits<NodeData>::getAxis( *mNodeData[nodeNum].
first, k );
300 *resultIndex = mNodeData[nodeNum].second;
static float getAxis0(const Vec2f &data)
Definition: KdTree.h:126
static float getAxis0(const NodeData &data)
Definition: KdTree.h:108
uint32_t hasLeftChild
Definition: KdTree.h:53
GLuint start
Definition: GLee.h:963
static float getAxis2(const NodeData &data)
Definition: KdTree.h:110
int int * max
Definition: GLee.h:17208
static float getAxis1(const NodeData &data)
Definition: KdTree.h:109
void initLeaf()
Definition: KdTree.h:45
GLsizei GLsizei GLenum GLenum const GLvoid * data
Definition: GLee.h:1011
T x
Definition: Vector.h:71
KdTree()
Definition: KdTree.h:69
static float distanceSquared(const NodeData &data, float k[3])
Definition: KdTree.h:111
~KdTree()
Definition: KdTree.h:72
#define min(a, b)
Definition: AppImplMsw.cpp:36
static uint32_t getSize(const NDV &ndv)
Definition: KdTree.h:95
GLint * first
Definition: GLee.h:1725
uint32_t splitAxis
Definition: KdTree.h:52
static float getAxis(const NodeData &data, int axis)
Definition: KdTree.h:103
void findNearest(float p[K], float result[K], uint32_t *resultIndex) const
Definition: KdTree.h:262
std::pair< const NodeData *, uint32_t > NodeDataIndex
Definition: KdTree.h:64
void initialize(const NodeDataVector &d)
Definition: KdTree.h:155
static float distanceSquared(const Vec2f &data, float k[2])
Definition: KdTree.h:128
const GLdouble * v
Definition: GLee.h:1384
T y
Definition: Vector.h:71
GLuint GLuint end
Definition: GLee.h:963
void lookup(const NodeData &p, const LookupProc &process, float maxDist) const
Definition: KdTree.h:218
GLfloat GLfloat p
Definition: GLee.h:8473
static float getAxis1(const Vec2f &data)
Definition: KdTree.h:127
CompareNode(int a)
Definition: KdTree.h:136
void process(uint32_t id, float distSqrd, float &maxDistSqrd)
Definition: KdTree.h:59
void init(float p, uint32_t a)
Definition: KdTree.h:39
GLboolean GLboolean GLboolean GLboolean a
Definition: GLee.h:2964
static float getAxis(const Vec2f &data, int axis)
Definition: KdTree.h:122
void recursiveBuild(uint32_t nodeNum, uint32_t start, uint32_t end, std::vector< NodeDataIndex > &buildNodes)
Definition: KdTree.h:170
bool operator()(const std::pair< const NodeData *, uint32_t > &d1, const std::pair< const NodeData *, uint32_t > &d2) const
Definition: KdTree.h:138
float splitPos
Definition: KdTree.h:51
uint32_t rightChild
Definition: KdTree.h:54
int axis
Definition: KdTree.h:137