10 /** Routines for computing with T[length] number fields */
12 /** Calls transformer(x,y) for every x from [i1..iEnd1) and corresponding y from [i2..) */
13 template<class I1
,class I2
,class Transf
>
14 Transf
transform2( I1 i1
, I1 iEnd1
, I2 i2
, Transf transformer
) {
15 for (; i1
!=iEnd1
; ++i1
,++i2
)
19 /** Analogous to ::transform2 */
20 template<class I1
,class I2
,class I3
,class Transf
>
21 Transf
transform3( I1 i1
, I1 iEnd1
, I2 i2
, I3 i3
, Transf transformer
) {
22 for (; i1
!=iEnd1
; ++i1
,++i2
,++i3
)
23 transformer(*i1
,*i2
,*i3
);
26 /** Analogous to ::transform2 */
27 template<class I1
,class I2
,class I3
,class I4
,class Transf
>
28 Transf
transform4( I1 i1
, I1 iEnd1
, I2 i2
, I3 i3
, I4 i4
, Transf transformer
) {
29 for (; i1
!=iEnd1
; ++i1
,++i2
,++i3
,++i4
)
30 transformer(*i1
,*i2
,*i3
,*i4
);
34 /** Means b[i]=a[i]; */
35 template<class T
> T
* assign(const T
*a
,int length
,T
*b
) {
38 memcpy(b
,a
,length
*sizeof(T
));
44 template<class T
,bool CheckNaNs
> struct MoveToBounds
{
45 T sqrError
; // TODO: double?
50 void operator()(const T
&point
,const T bounds
[2],T
&result
) {
51 if ( CheckNaNs
&& isNaN(point
) )
53 if ( point
< bounds
[0] ) {
54 sqrError
+= sqr(bounds
[0]-point
);
57 if ( point
> bounds
[1] ) {
58 sqrError
+= sqr(point
-bounds
[1]);
65 /** Copy_moves a vector (\p point) to the nearest point (\p result)
66 * within bounds (\p bounds) and returns SE (distance^2) */
67 template<class T
,bool CheckNaNs
>
68 T
moveToBounds_copy(const T
*point
,const T (*bounds
)[2],int length
,T
*result
) {
69 return transform3( point
, point
+length
, bounds
, result
70 , MoveToBounds
<T
,CheckNaNs
>() ) .sqrError
;
72 // namespace NOSPACE {
73 // template<class T> struct Min {
74 // const T& operator()(const T &a,const T &b) const
75 // { return min(a,b); }
77 // template<class T> struct Max {
78 // const T& operator()(const T &a,const T &b) const
79 // { return max(a,b); }
82 // /** Means b[i]=min(a[i],b[i]) */
83 // template<class T> T* min(const T *a,int length,T *b) {
84 // transform( a, a+length, b, b, Min<T>() );
87 // /** Means b[i]=max(a[i],b[i]) */
88 // template<class T> T* max(const T *a,int length,T *b) {
89 // transform( a, a+length, b, b, Max<T>() );
93 // /** Returns whether a[i]<=b[i] */
94 // template<class T> bool isOrdered(const T *a,const T *b,int length) {
95 // const T *aEnd=a+length;
104 template<class T> struct Adder {
105 void operator()(const T &a,const T &b,T &c)
108 template<class T> struct Subtractor {
109 void operator()(const T &a,const T &b,T &c)
113 template<class T> T* add(const T *a,const T *b,int length,T *c) {
114 transform3( a, a+length, b, c, Adder<T>() );
116 template<class T> T* subtract(const T *a,const T *b,int length,T *c) {
117 transform3( a, a+length, b, c, Subtractor<T>() );
122 template<class T
> class KDBuilder
;
123 /** A generic KD-tree */
124 template<class T
> class KDTree
{
126 friend class KDBuilder
<T
>;
127 typedef KDBuilder
<T
> Builder
;
129 /** Represents one node of the KD-tree */
131 int coord
; ///< The coordinate along which the tree splits in this node
132 T threshold
;///< The threshold of the split (left[#coord] <= #threshold <= right[#coord])
134 typedef T (*Bounds
)[2];
137 const int depth
/// The depth of the tree = ::log2ceil(#count)
138 , length
/// The length of the vectors
139 , count
; ///< The number of the vectors
141 Node
*nodes
; ///< The array of the tree-nodes
142 int *dataIDs
; ///< Data IDs for children of "leaf" nodes
143 Bounds bounds
; ///< The bounding box for all the data
145 /** Builds a new KD-tree from \p count_ vectors of \p length_ elements stored in \data */
146 KDTree(int length_
,int count_
)
147 : depth( log2ceil(count_
) ), length(length_
), count(count_
)
148 , nodes( new Node
[count_
] ), dataIDs( new int[count_
] ), bounds( new T
[length_
][2] ) {
151 /** Copy constructor with swapping (or rather moving) semantics */
152 KDTree(KDTree
&other
)
153 : depth(other
.depth
), length(other
.length
), count(other
.count
)
154 , nodes(other
.nodes
), dataIDs(other
.dataIDs
), bounds(other
.bounds
) {
161 /** Only frees the memory */
168 /** Takes an index of "leaf" (nonexistent) node and returns the appropriate data ID */
169 int leafID2dataID(int leafID
) const {
170 ASSERT( count
<=leafID
&& leafID
<2*count
);
171 int index
= leafID
-powers
[depth
];
174 ASSERT( 0<=index
&& index
<count
);
175 return dataIDs
[index
];
178 /** Manages a heap from nodes of a KDTree.
179 * It returns vectors (their indices) in the order according to distance (SE)
180 * from a given fixed point. It can return a lower bound of the SEs of the remaining
181 * vectors at any time. */
183 /** One element of the heap representing a node in the KDTree */
185 int nodeIndex
; ///< Index of the node in #kd
186 T
*data
; ///< Stores the SE and the coordinates of the nearest point
188 /** No-init constructor */
190 /** Initializes the members from the parameters */
191 HeapNode(int nodeIndex_
,T
*data_
): nodeIndex(nodeIndex_
), data(data_
) {}
193 /** Returns reference to the SE of the nearest point to this node's bounding box */
194 T
& getSE() { return *data
; }
195 /** Returns the SE of the nearest point to this node's bounding box */
196 T
getSE() const { return *data
; }
197 /** Returns pointer to the nearest point to this node's bounding box */
198 T
* getNearest() { return data
+1; }
200 /** Defines the order of #heap - ascending according to #getSE */
202 bool operator()(const HeapNode
&a
,const HeapNode
&b
)
203 { return a
.getSE() > b
.getSE(); }
206 const KDTree
&kd
; ///< Reference to the KDTree we operate on
207 const T
* const point
; ///< Pointer to the point we are trying to approach
208 vector
<HeapNode
> heap
; ///< The current heap of the tree nodes
209 BulkAllocator
<T
> allocator
; ///< The allocator for HeapNode::data
211 /** Build the heap from the KDTree \p tree and vector \p point_
212 * (they've got to remain valid until destruction) */
213 PointHeap(const KDTree
&tree
,const T
*point_
,bool checkNaNs
)
214 : kd(tree
), point(point_
) {
216 // create the root heap-node
217 HeapNode
rootNode( 1, allocator
.makeField(kd
.length
+1) );
218 // compute the nearest point within the global bounds and corresponding SE
219 using namespace FieldMath
;
220 rootNode
.getSE()= checkNaNs
221 ? moveToBounds_copy
<T
,true> ( point
, kd
.bounds
, kd
.length
, rootNode
.getNearest() )
222 : moveToBounds_copy
<T
,false>( point
, kd
.bounds
, kd
.length
, rootNode
.getNearest() );
223 // push it onto the heap (and reserve more to speed up the first leaf-gettings)
224 heap
.reserve(kd
.depth
*2);
225 heap
.push_back(rootNode
);
228 /** Returns whether the heap is empty ( !isEmpty() is needed for all other methods) */
230 { return heap
.empty(); }
232 /** Returns the SE of the top node (always equals the SE of the next leaf) */
234 ASSERT( !isEmpty() );
235 return heap
[0].getSE();
238 /** Removes a leaf, returns the matching vector's index */
239 template<bool CheckNaNs
> int popLeaf() {
240 // ensure a leaf is on the top of the heap and get its ID
241 makeTopLeaf
<CheckNaNs
>();
242 int result
= kd
.leafID2dataID( heap
.front().nodeIndex
);
243 // remove the top from the heap (no need to free the memory - #allocator)
244 pop_heap( heap
.begin(), heap
.end(), HeapOrder() );
249 /** Divides the top nodes until there's a leaf on the top */
250 template<bool CheckNaNs
> void makeTopLeaf();
252 }; // PointHeap class
256 template<class T
> template<bool CheckNaNs
>
257 void KDTree
<T
>::PointHeap::makeTopLeaf() {
258 ASSERT( !isEmpty() );
259 // exit if there's a leaf on the top already
260 if ( !(heap
[0].nodeIndex
<kd
.count
) )
262 PtrInt oldHeapSize
= heap
.size();
263 HeapNode heapRoot
= heap
[0]; // making a local working copy of the top of the heap
265 do { // while heapRoot isn't leaf ... while ( heapRoot.nodeIndex<kd.count )
266 const Node
&node
= kd
.nodes
[heapRoot
.nodeIndex
];
267 // now replace the node with its two children:
268 // one of them will have the same SE (replaces its parent on the top),
269 // the other one can have higher SE (push_back-ed on the heap)
271 // create a new heap-node and allocate it's data
273 newHNode
.data
= allocator
.makeField(kd
.length
+1);
274 // assign index of the left child for both the heap-nodes (for now)
275 newHNode
.nodeIndex
= (heapRoot
.nodeIndex
*= 2);
276 // the nearest point of the new heap-node only differs in one coordinate
277 FieldMath::assign( heapRoot
.getNearest(), kd
.length
, newHNode
.getNearest() );
279 if ( !CheckNaNs
|| !isNaN(point
[node
.coord
]) ) { // valid coordinate -> normal processing
280 newHNode
.getNearest()[node
.coord
]= node
.threshold
;
282 // the SE of the child heap-nodes can be computed from the parent heap-node
283 Real oldDistance
= Real(point
[node
.coord
]) - heapRoot
.getNearest()[node
.coord
];
284 Real newDistance
= Real(point
[node
.coord
]) - node
.threshold
;
285 newHNode
.getSE()= heapRoot
.getSE() - sqr(oldDistance
) + sqr(newDistance
);
286 ASSERT( newHNode
.getSE() >= heapRoot
.getSE() );
287 /* another way to do this, according to: A^2-B^2 = (A-B)*(A+B)
288 Real nearestInCoord= heapRoot.getNearest()[node.coord];
289 newHNode.getSE()= heapRoot.getSE() + (nearestInCoord-node.threshold)
290 * ( ldexp((Real)point[node.coord],1) - nearestInCoord - node.threshold );
292 // correct the nodeIndex of the new children
293 if ( newDistance
<= 0 )
294 // the point is in the left part -> the left child will be on the top
295 ++newHNode
.nodeIndex
;
297 // the point is in the right part -> the right child will be on the top
298 ++heapRoot
.nodeIndex
;
300 } else { // the coordinate doesn't have any effect
301 newHNode
.getSE()= heapRoot
.getSE();
302 ++newHNode
.nodeIndex
;
305 // add the new node to the back, restore the heap-property later
306 heap
.push_back(newHNode
);
307 } while ( heapRoot
.nodeIndex
<kd
.count
);
309 heap
[0]= heapRoot
; // restoring the working copy of the heap's top node
310 // restore the heap-property on the added nodes
311 typename vector
<HeapNode
>::iterator it
= heap
.begin()+oldHeapSize
;
313 push_heap( heap
.begin(), it
, HeapOrder() );
314 while ( it
++ != heap
.end() );
315 } // KDTree<T>::PointHeap::makeTopLeaf<CheckNaNs>() method
318 template<class T
> class KDBuilder
: public KDTree
<T
> {
320 typedef KDTree
<T
> Tree
;
321 typedef T BoundsPair
[2];
322 typedef typename
Tree::Bounds Bounds
;
323 typedef int (KDBuilder::*CoordChooser
)
324 (int nodeIndex
,int *beginIDs
,int *endIDs
,int depthLeft
) const;
326 using Tree::depth
; using Tree::length
; using Tree::count
;
327 using Tree::nodes
; using Tree::dataIDs
; using Tree::bounds
;
330 const CoordChooser chooser
;
331 mutable Bounds chooserTmp
;
333 KDBuilder(const T
*data_
,int length
,int count
,CoordChooser chooser_
)
334 : Tree(length
,count
), data(data_
), chooser(chooser_
), chooserTmp(0) {
335 ASSERT( length
>0 && count
>0 && chooser
&& data
);
336 // create the index-vector, coumpute the bounding box, build the tree
337 for (int i
=0; i
<count
; ++i
)
341 buildNode(1,dataIDs
,dataIDs
+count
,depth
);
343 DEBUG_ONLY( chooserTmp
= 0; )
346 /** Creates bounds containing one value */
348 void operator()(const T
&val
,BoundsPair
&bounds
) const
349 { bounds
[0]= bounds
[1]= val
; }
351 /** Expands a valid bounding box to contain a point (one coordinate at once) */
352 struct BoundsExpander
{
353 void operator()(const T
&val
,BoundsPair
&bounds
) const {
354 if ( val
< bounds
[0] ) // lower bound
356 if ( val
> bounds
[1] ) // upper bound
360 /** Computes the bounding box of #count vectors with length #length stored in #data.
361 * The vectors in #data are stored linearly, /p boundsRes should be preallocated */
362 void getBounds(Bounds boundsRes
) const {
363 using namespace FieldMath
;
365 // make the initial bounds only contain the first point
366 transform2(data
,data
+length
,boundsRes
,NewBounds());
367 int count
= Tree::count
;
368 const T
*nowData
= data
;
369 // expand the bounds by every point (except for the first one)
372 transform2( nowData
, nowData
+length
, boundsRes
, BoundsExpander() );
375 /** Like #getBounds, but it only works on vectors with indices from [\p beginIDs..\p endIDs)
376 * instead of [0..#count), \p boundsRes should be preallocated to store the result */
377 void getBounds(const int *beginIDs
,const int *endIDs
,Bounds boundsRes
) const {
378 using namespace FieldMath
;
379 ASSERT(endIDs
>beginIDs
);
380 // make the initial bounds only contain the first point
381 const T
*begin
= data
+ *beginIDs
*length
;
382 transform2(begin
,begin
+length
,boundsRes
,NewBounds());
383 // expand the bounds by every point (except for the first one)
384 while ( ++beginIDs
!= endIDs
) {
385 begin
= data
+ *beginIDs
*length
;
386 transform2( begin
, begin
+length
, boundsRes
, BoundsExpander() );
390 /** Recursively builds node \p nodeIndex and its subtree of depth \p depthLeft
391 * (including leaves), operates on data \p data in the range [\p beginIDs,\p endIDs) */
392 void buildNode(int nodeIndex
,int *beginIDs
,int *endIDs
,int depthLeft
);
395 /** CoordChooser choosing the longest coordinate of the bounding box of the current interval */
396 int choosePrecise(int nodeIndex
,int *beginIDs
,int *endIDs
,int /*depthLeft*/) const;
397 /** CoordChooser choosing the coordinate only according to the depth */
398 int chooseFast(int /*nodeIndex*/,int* /*beginIDs*/,int* /*endIDs*/,int depthLeft
) const
399 { return depthLeft
%length
; }
400 /** CoordChooser choosing a random coordinate */
401 int chooseRand(int /*nodeIndex*/,int* /*beginIDs*/,int* /*endIDs*/,int /*depthLeft*/) const
402 { return rand()%length
; }
403 int chooseApprox(int nodeIndex
,int* /*beginIDs*/,int* /*endIDs*/,int depthLeft
) const;
405 static Tree
* makeTree(const T
*data
,int length
,int count
,CoordChooser chooser
) {
406 KDBuilder
builder(data
,length
,count
,chooser
);
407 return new Tree(builder
);
409 }; // KDBuilder class
414 /** Finds the longest coordinate of a bounding box */
415 template<class T
> struct MaxDiffCoord
{
416 typedef typename KDBuilder
<T
>::BoundsPair BoundsPair
;
419 int bestIndex
, nextIndex
;
421 MaxDiffCoord(const BoundsPair
& bounds0
)
422 : maxDiff(bounds0
[1]-bounds0
[0]), bestIndex(0), nextIndex(1) {}
424 void operator()(const BoundsPair
& bounds_i
) {
425 T diff
= bounds_i
[1]-bounds_i
[0];
427 bestIndex
= nextIndex
;
434 template<class T
> int KDBuilder
<T
>
435 ::choosePrecise(int nodeIndex
,int *beginIDs
,int *endIDs
,int) const {
436 ASSERT( nodeIndex
>0 && beginIDs
&& endIDs
&& beginIDs
<endIDs
);
437 // temporary storage for computed bounding box
438 BoundsPair boundsStorage
[length
];
439 const BoundsPair
*localBounds
;
441 if ( nodeIndex
>1 ) { // compute the bounding box
442 localBounds
= boundsStorage
;
443 getBounds( beginIDs
, endIDs
, boundsStorage
);
444 } else // we are in the root -> we can use already computed bounds
446 // find and return the longest coordinate
447 MaxDiffCoord
<T
> mdc
= for_each
448 ( localBounds
+1, localBounds
+length
, MaxDiffCoord
<T
>(localBounds
[0]) );
449 ASSERT( mdc
.nextIndex
== length
);
450 return mdc
.bestIndex
;
452 template<class T
> int KDBuilder
<T
>::chooseApprox(int nodeIndex
,int*,int*,int) const {
453 using namespace FieldMath
;
456 int myDepth
= log2ceil(nodeIndex
+1)-1;
457 if (!myDepth
) { // I'm in the root - copy the bounds
458 ASSERT(nodeIndex
==1);
459 chooserTmp
= new BoundsPair
[length
*(depth
+1)]; // allocate my temporary bound-array
460 assign( bounds
, length
, chooserTmp
);
463 Bounds myBounds
= chooserTmp
+length
*myDepth
;
464 if (myDepth
) { // I'm not the root - copy parent's bounds and modify them
465 const typename
Tree::Node
&parent
= nodes
[nodeIndex
/2];
466 Bounds parentBounds
= myBounds
-length
;
467 if (nodeIndex
%2) { // I'm the right son -> bounds on this level not initialized
468 assign( parentBounds
, length
, myBounds
);
469 myBounds
[parent
.coord
][0]= parent
.threshold
; // adjusting the lower bound
470 } else // I'm the left son
471 if ( nodeIndex
+1 < count
) { // almost the same as brother -> only adjust the coordinate
472 myBounds
[parent
.coord
][0]= parentBounds
[parent
.coord
][0];
473 myBounds
[parent
.coord
][1]= parent
.threshold
;
474 } else { // I've got no brother
475 ASSERT( nodeIndex
+1 == count
);
476 assign( parentBounds
, length
, myBounds
);
477 myBounds
[parent
.coord
][1]= parent
.threshold
;
480 // find out the widest dimension
481 MaxDiffCoord
<T
> mdc
= for_each( myBounds
+1, myBounds
+length
, MaxDiffCoord
<T
>(myBounds
[0]) );
482 ASSERT( mdc
.nextIndex
== length
);
483 return mdc
.bestIndex
;
487 /** Compares vectors (given by their indices) according to a given coordinate */
488 template<class T
> class IndexComparator
{
492 IndexComparator(const T
*data_
,int length_
,int index_
)
493 : data(data_
+index_
), length(length_
) {}
494 bool operator()(int a
,int b
) const
495 { return data
[a
*length
] < data
[b
*length
]; }
498 template<class T
> void KDBuilder
<T
>
499 ::buildNode(int nodeIndex
,int *beginIDs
,int *endIDs
,int depthLeft
) {
500 int count
= endIDs
-beginIDs
; // owershadowing Tree::count
501 // check we've got at least one vector and the depth&count are adequate to each other
502 ASSERT( count
>=2 && powers
[depthLeft
-1]<count
&& count
<=powers
[depthLeft
] );
504 bool shallowRight
= ( count
<= powers
[depthLeft
]+powers
[depthLeft
-1] );
505 int *middle
= shallowRight
506 ? endIDs
-powers
[depthLeft
-1]
507 : beginIDs
+powers
[depthLeft
];
508 // find out the dividing coordinate and find the median in this coordinate
509 int coord
= (this->*chooser
)(nodeIndex
,beginIDs
,endIDs
,depthLeft
);
510 nth_element( beginIDs
, middle
, endIDs
, IndexComparator
<T
>(data
,length
,coord
) );
511 // fill the node's data (dividing coordinate and its threshold)
512 nodes
[nodeIndex
].coord
= coord
;
513 nodes
[nodeIndex
].threshold
= data
[*middle
*length
+coord
];
514 // recurse on both halves (if needed; fall-through switch)
516 default: // we've got enough nodes - build both subtrees (fall through)
517 // build the right subtree
518 buildNode( 2*nodeIndex
+1, middle
, endIDs
, depthLeft
-shallowRight
);
519 case 3: // only a pair in the first half
520 // build the left subtree
521 buildNode( 2*nodeIndex
, beginIDs
, middle
, depthLeft
);
522 case 2: // nothing needs to be sorted
527 #endif // KDTREE_HEADER_