Cinder  0.8.6
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Macros Pages
KdTree.h
Go to the documentation of this file.
1 /*
2  Copyright (c) 2009, The Barbarian Group
3  All rights reserved.
4 
5  Redistribution and use in source and binary forms, with or without modification, are permitted provided that
6  the following conditions are met:
7 
8  * Redistributions of source code must retain the above copyright notice, this list of conditions and
9  the following disclaimer.
10  * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and
11  the following disclaimer in the documentation and/or other materials provided with the distribution.
12 
13  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED
14  WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
15  PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
16  ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
17  TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
18  HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
19  NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
20  POSSIBILITY OF SUCH DAMAGE.
21 */
22 
23 #pragma once
24 
25 #include "cinder/Cinder.h"
26 #include "cinder/Vector.h"
27 
28 #include <vector>
29 #include <float.h>
30 #include <stdlib.h>
31 #include <algorithm>
32 #include <utility>
33 
34 namespace cinder {
35 
36 // KdTree Declarations
37 template<unsigned char K>
38 struct KdNode {
39  void init( float p, uint32_t a) {
40  splitPos = p;
41  splitAxis = a;
42  rightChild = ~0;
43  hasLeftChild = 0;
44  }
45  void initLeaf() {
46  splitAxis = K;
47  rightChild = ~0;
48  hasLeftChild = 0;
49  }
50  // KdNode Data
51  float splitPos;
52  uint32_t splitAxis:2;
53  uint32_t hasLeftChild:1;
54  uint32_t rightChild:29;
55 };
56 
58  public:
59  void process( uint32_t id, float distSqrd, float &maxDistSqrd ) {}
60 };
61 
62 template <typename NodeData, unsigned char K=3, class LookupProc = NullLookupProc> class KdTree {
63 public:
64  typedef std::pair<const NodeData*, uint32_t> NodeDataIndex;
65 
66  // KdTree Public Methods
67  template<typename NodeDataVector>
68  KdTree( const NodeDataVector &data );
69  KdTree() {}
70  template<typename NodeDataVector>
71  void initialize( const NodeDataVector &d );
72  ~KdTree() {
73  free( nodes );
74  delete[] mNodeData;
75  }
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;
79 
80 private:
81  // KdTree Private Methods
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;
84  // KdTree Private Data
85  KdNode<K> *nodes;
86  NodeDataIndex *mNodeData;
87  uint32_t nNodes, nextFreeNode;
88 };
89 
90 
91 // Shims
92 template<typename NDV>
94 {
95  static uint32_t getSize( const NDV &ndv ) {
96  return static_cast<uint32_t>( ndv.size() );
97  }
98 };
99 
100 template<typename NodeData>
102 {
103  static float getAxis( const NodeData &data, int axis ) {
104  if( axis == 0 ) return data.x;
105  else if( axis == 1 ) return data.y;
106  else return (float)data.z;
107  }
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 ); }
111  static float distanceSquared( const NodeData &data, float k[3] ) {
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] );
115  return result;
116  }
117 };
118 
119 template<>
121 {
122  static float getAxis( const Vec2f &data, int axis ) {
123  if( axis == 0 ) return data.x;
124  else return data.y;
125  }
126  static float getAxis0( const Vec2f &data ) { return static_cast<float>( data.x ); }
127  static float getAxis1( const Vec2f &data ) { return static_cast<float>( data.y ); }
128  static float distanceSquared( const Vec2f &data, float k[2] ) {
129  float result = ( data.x - k[0] ) * ( data.x - k[0] );
130  result += ( data.y - k[1] ) * ( data.y - k[1] );
131  return result;
132  }
133 };
134 
135 template<typename NodeData> struct CompareNode {
136  CompareNode( int a ) { axis = a; }
137  int axis;
138  bool operator()(const std::pair<const NodeData*,uint32_t> &d1,
139  const std::pair<const NodeData*,uint32_t> &d2) const {
140  return NodeDataTraits<NodeData>::getAxis( *d1.first, axis ) == NodeDataTraits<NodeData>::getAxis( *d2.first, axis ) ? ( d1.first < d2.first ) :
142  }
143 };
144 
145 // KdTree Method Definitions
146 template<typename NodeData, unsigned char K, typename LookupProc>
147  template<typename NodeDataVector>
149 {
150  initialize( d );
151 }
152 
153 template<typename NodeData, unsigned char K, typename LookupProc>
154  template<typename NodeDataVector>
155 void KdTree<NodeData, K, LookupProc>::initialize( const NodeDataVector &d )
156 {
158  nextFreeNode = 1;
159  nodes = (KdNode<K> *)malloc(nNodes * sizeof(KdNode<K>));
160  mNodeData = new NodeDataIndex[nNodes];
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 ) );
165  // Begin the KdTree building process
166  recursiveBuild( 0, 0, nNodes, buildNodes );
167 }
168 
169 template<typename NodeData, unsigned char K, typename LookupProc>
170 void KdTree<NodeData, K, LookupProc>::recursiveBuild( uint32_t nodeNum, uint32_t start, uint32_t end, std::vector<NodeDataIndex> &buildNodes )
171 {
172  // Create leaf node of kd-tree if we've reached the bottom
173  if( start + 1 == end) {
174  nodes[nodeNum].initLeaf();
175  mNodeData[nodeNum] = buildNodes[start];
176  return;
177  }
178  // Choose split direction and partition data
179  // Compute bounds of data from _start_ to _end_
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;
184  }
185 
186  for( uint32_t i = start; i < end; ++i ) {
187  for( uint8_t axis = 0; axis < K; axis++ ) {
188  // NOT Compiling? you should define NOMINMAX
189  boundMin[axis] = std::min( boundMin[axis], NodeDataTraits<NodeData>::getAxis( *buildNodes[i].first, axis ) );
190  boundMax[axis] = std::max( boundMax[axis], NodeDataTraits<NodeData>::getAxis( *buildNodes[i].first, axis ) );
191  }
192  }
193  int splitAxis = 0;
194  float maxExtent = boundMax[0] - boundMin[0];
195  for( unsigned char k = 1; k < K; ++k ) {
196  if( boundMax[k] - boundMin[k] > maxExtent ) {
197  splitAxis = k;
198  maxExtent = boundMax[k] - boundMin[k];
199  }
200  }
201  uint32_t splitPos = ( start + end ) / 2;
202  std::nth_element( &buildNodes[start], &buildNodes[splitPos], &buildNodes[end-1] + 1, CompareNode<NodeData>(splitAxis) );
203  // Allocate kd-tree node and continue recursively
204  nodes[nodeNum].init( NodeDataTraits<NodeData>::getAxis( *buildNodes[splitPos].first, splitAxis ), 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 );
210  }
211  if( splitPos + 1 < end ) {
212  nodes[nodeNum].rightChild = nextFreeNode++;
213  recursiveBuild( nodes[nodeNum].rightChild, splitPos + 1, end, buildNodes );
214  }
215 }
216 
217 template<typename NodeData, unsigned char K, typename LookupProc>
218 void KdTree<NodeData, K, LookupProc>::lookup( const NodeData &p, const LookupProc &proc, float maxDist ) const
219 {
220  float maxDistSqrd = maxDist * maxDist;
221  float pt[K];
222  for( unsigned char k = 0; k < K; ++k )
223  pt[k] = NodeDataTraits<NodeData>::getAxis( p, k );
224 
225  privateLookup( 0, pt, proc, maxDistSqrd );
226 }
227 
228 template<typename NodeData, unsigned char K, typename LookupProc>
229 void KdTree<NodeData, K, LookupProc>::privateLookup( uint32_t nodeNum, float p[K], const LookupProc &process, float &maxDistSquared ) const
230 {
231  KdNode<K> *node = &nodes[nodeNum];
232  // process kd-tree node's children
233  int axis = node->splitAxis;
234  if( axis != K ) {
235  float dist2 = ( p[axis] - node->splitPos ) * ( p[axis] - node->splitPos );
236  if( p[axis] <= node->splitPos ) {
237  if(node->hasLeftChild)
238  privateLookup( nodeNum + 1, p, process, maxDistSquared );
239  if( ( dist2 < maxDistSquared ) && ( node->rightChild < nNodes ) )
240  privateLookup( node->rightChild, p, process, maxDistSquared );
241  }
242  else {
243  if( node->rightChild < nNodes )
244  privateLookup( node->rightChild, p, process, maxDistSquared );
245  if( ( dist2 < maxDistSquared ) && node->hasLeftChild )
246  privateLookup( nodeNum + 1, p, process, maxDistSquared );
247  }
248  }
249  // Hand kd-tree node to processing function
250  float distSqr = 0.0f;
251  for( unsigned char k = 0; k < K; ++k ) {
252  float v = NodeDataTraits<NodeData>::getAxis( *mNodeData[nodeNum].first, k ) - p[k];
253  distSqr += v * v;
254  }
255 
256  if( distSqr < maxDistSquared )
257  process.process( mNodeData[nodeNum].second, distSqr, maxDistSquared );
258 }
259 
260 // Find Nearest
261 template<typename NodeData, unsigned char K, typename LookupProc>
262 void KdTree<NodeData, K, LookupProc>::findNearest( float p[K], float result[K], uint32_t *resultIndex ) const
263 {
264  float maxDist = FLT_MAX;
265  *resultIndex = -1;
266  privateFindNearest( 0, p, maxDist, result, resultIndex );
267 }
268 
269 template<typename NodeData, unsigned char K, typename LookupProc>
270 void KdTree<NodeData, K, LookupProc>::privateFindNearest( uint32_t nodeNum, float p[K], float &maxDistSquared, float result[K], uint32_t *resultIndex ) const
271 {
272  KdNode<K> *node = &nodes[nodeNum];
273  // process kd-tree node's children
274  int axis = node->splitAxis;
275  if( axis != K ) {
276  float dist2 = (p[axis] - node->splitPos) * (p[axis] - node->splitPos);
277  if( p[axis] <= node->splitPos ) {
278  if( node->hasLeftChild )
279  privateFindNearest( nodeNum+1, p, maxDistSquared, result, resultIndex );
280  if( ( dist2 < maxDistSquared ) && ( node->rightChild < nNodes) )
281  privateFindNearest( node->rightChild, p, maxDistSquared, result, resultIndex );
282  }
283  else {
284  if( node->rightChild < nNodes)
285  privateFindNearest(node->rightChild,
286  p,
287  maxDistSquared, result, resultIndex );
288  if( dist2 < maxDistSquared && node->hasLeftChild)
289  privateFindNearest(nodeNum+1,
290  p,
291  maxDistSquared, result, resultIndex );
292  }
293  }
294 
295  float distSqr = NodeDataTraits<NodeData>::distanceSquared( *mNodeData[nodeNum].first, p );
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;
301  }
302 }
303 
304 } // namespace ci
Definition: KdTree.h:93
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
Definition: KdTree.h:57
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
Definition: KdTree.h:135
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
Definition: KdTree.h:38
static float distanceSquared(const Vec2f &data, float k[2])
Definition: KdTree.h:128
Definition: KdTree.h:62
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
Definition: KdTree.h:101