kD-tree

[cpp] view plaincopyprint?

1. kdtree的原理就是基于二叉树的形式，将高维空间用超矩形进行划分.其主要用途是用来求解高维空间中最近邻的值.  

 

 

下面是kdtree.h文件，是kdtree数据结构的头文件

[cpp] view plaincopyprint?

1. #ifndef _KDTREE_H_  
2. #define _KDTREE_H_  
3.   
4. #ifdef __cplusplus  
5. extern "C" {  
6. #endif  
7.   
8. struct kdtree;  
9. struct kdres;  
10.   
11.   
12. /* create a kd-tree for "k"-dimensional data */  
13. struct kdtree *kd_create(int k);  
14.   
15. /* free the struct kdtree */  
16. void kd_free(struct kdtree *tree);  
17.   
18. /* remove all the elements from the tree */  
19. void kd_clear(struct kdtree *tree);  
20.   
21. /* if called with non-null 2nd argument, the function provided 
22.  * will be called on data pointers (see kd_insert) when nodes 
23.  * are to be removed from the tree. 
24.  */  
25. void kd_data_destructor(struct kdtree *tree, void (*destr)(void*));  
26.   
27. /* insert a node, specifying its position, and optional data */  
28. int kd_insert(struct kdtree *tree, const double *pos, void *data);  
29. int kd_insertf(struct kdtree *tree, const float *pos, void *data);  
30. int kd_insert3(struct kdtree *tree, double x, double y, double z, void *data);  
31. int kd_insert3f(struct kdtree *tree, float x, float y, float z, void *data);  
32.   
33. /* Find the nearest node from a given point. 
34.  * 
35.  * This function returns a pointer to a result set with at most one element. 
36.  */  
37. struct kdres *kd_nearest(struct kdtree *tree, const double *pos);  
38. struct kdres *kd_nearestf(struct kdtree *tree, const float *pos);  
39. struct kdres *kd_nearest3(struct kdtree *tree, double x, double y, double z);  
40. struct kdres *kd_nearest3f(struct kdtree *tree, float x, float y, float z);  
41.   
42. /* Find the N nearest nodes from a given point. 
43.  * 
44.  * This function returns a pointer to a result set, with at most N elements, 
45.  * which can be manipulated with the kd_res_* functions. 
46.  * The returned pointer can be null as an indication of an error. Otherwise 
47.  * a valid result set is always returned which may contain 0 or more elements. 
48.  * The result set must be deallocated with kd_res_free after use. 
49.  */  
50. /* 
51. struct kdres *kd_nearest_n(struct kdtree *tree, const double *pos, int num); 
52. struct kdres *kd_nearest_nf(struct kdtree *tree, const float *pos, int num); 
53. struct kdres *kd_nearest_n3(struct kdtree *tree, double x, double y, double z); 
54. struct kdres *kd_nearest_n3f(struct kdtree *tree, float x, float y, float z); 
55. */  
56.   
57. /* Find any nearest nodes from a given point within a range. 
58.  * 
59.  * This function returns a pointer to a result set, which can be manipulated 
60.  * by the kd_res_* functions. 
61.  * The returned pointer can be null as an indication of an error. Otherwise 
62.  * a valid result set is always returned which may contain 0 or more elements. 
63.  * The result set must be deallocated with kd_res_free after use. 
64.  */  
65. struct kdres *kd_nearest_range(struct kdtree *tree, const double *pos, double range);  
66. struct kdres *kd_nearest_rangef(struct kdtree *tree, const float *pos, float range);  
67. struct kdres *kd_nearest_range3(struct kdtree *tree, double x, double y, double z, double range);  
68. struct kdres *kd_nearest_range3f(struct kdtree *tree, float x, float y, float z, float range);  
69.   
70. /* frees a result set returned by kd_nearest_range() */  
71. void kd_res_free(struct kdres *set);  
72.   
73. /* returns the size of the result set (in elements) */  
74. int kd_res_size(struct kdres *set);  
75.   
76. /* rewinds the result set iterator */  
77. void kd_res_rewind(struct kdres *set);  
78.   
79. /* returns non-zero if the set iterator reached the end after the last element */  
80. int kd_res_end(struct kdres *set);  
81.   
82. /* advances the result set iterator, returns non-zero on success, zero if 
83.  * there are no more elements in the result set. 
84.  */  
85. int kd_res_next(struct kdres *set);  
86.   
87. /* returns the data pointer (can be null) of the current result set item 
88.  * and optionally sets its position to the pointers(s) if not null. 
89.  */  
90. void *kd_res_item(struct kdres *set, double *pos);  
91. void *kd_res_itemf(struct kdres *set, float *pos);  
92. void *kd_res_item3(struct kdres *set, double *x, double *y, double *z);  
93. void *kd_res_item3f(struct kdres *set, float *x, float *y, float *z);  
94.   
95. /* equivalent to kd_res_item(set, 0) */  
96. void *kd_res_item_data(struct kdres *set);  
97.   
98.   
99. #ifdef __cplusplus  
100. }  
101. #endif  
102.   
103. #endif  /* _KDTREE_H_ */  

下面是kdtree.c 文件，上面有我自己精心整理的详细的汉语注释，这个版本的kdtree可直接拿来用

[cpp] view plaincopyprint?

1. //kd_tree.h  kd_tree的头文件  
2.   
3. #include "stdafx.h"  
4.   
5. //头文件  
6. #include <stdio.h>  
7. #include <stdlib.h>  
8. #include <string.h>  
9. #include <math.h>  
10. #include "kd_tree.h"  
11.   
12. #if defined(WIN32) || defined(__WIN32__)  
13. #include <malloc.h>  
14. #endif  
15.   
16. #ifdef USE_LIST_NODE_ALLOCATOR  
17.   
18. #ifndef NO_PTHREADS  
19. #include <pthread.h>  
20. #else  
21.   
22. #ifndef I_WANT_THREAD_BUGS  
23. #error "You are compiling with the fast list node allocator, with pthreads disabled! This WILL break if used from multiple threads."  
24. #endif  /* I want thread bugs */  
25.   
26. #endif  /* pthread support */  
27. #endif  /* use list node allocator */  
28.   
29.   
30. //超平面的结构体  
31. //包括一个属性的维数和每维坐标的最大和最小值构成的数组  
32. struct kdhyperrect {  
33.     int dim;  
34.     double *min, *max;              /* minimum/maximum coords */  
35. };  
36.   
37. //节点的结构体，也就是事例的结构体  
38. struct kdnode {  
39.     double *pos;  
40.     int dir;  
41.     void *data;  
42.   
43.     struct kdnode *left, *right;    /* negative/positive side */  
44. };  
45.   
46. //返回结果节点， 包括树的节点,距离值, 是一个单链表的形式  
47. struct res_node {  
48.     struct kdnode *item;  
49.     double dist_sq;  
50.     struct res_node *next;  
51. };  
52.   
53. //树有几个属性，一是维数，一是树根节点，一是超平面，一是销毁data的函数  
54. struct kdtree {  
55.     int dim;  
56.     struct kdnode *root;  
57.     struct kdhyperrect *rect;  
58.     void (*destr)(void*);  
59. };  
60.   
61. //kdtree的返回结果，包括kdtree，这是一个双链表的形式  
62. struct kdres {  
63.     struct kdtree *tree;  
64.     struct res_node *rlist, *riter;  //双链表?  
65.     int size;  
66. };  
67.   
68. //计算平方的宏定义,相当于函数  
69. #define SQ(x)           ((x) * (x))  
70.   
71.   
72. static void clear_rec(struct kdnode *node, void (*destr)(void*));  
73. static int insert_rec(struct kdnode **node, const double *pos, void *data, int dir, int dim);  
74. static int rlist_insert(struct res_node *list, struct kdnode *item, double dist_sq);  
75. static void clear_results(struct kdres *set);  
76.   
77. static struct kdhyperrect* hyperrect_create(int dim, const double *min, const double *max);  
78. static void hyperrect_free(struct kdhyperrect *rect);  
79. static struct kdhyperrect* hyperrect_duplicate(const struct kdhyperrect *rect);  
80. static void hyperrect_extend(struct kdhyperrect *rect, const double *pos);  
81. static double hyperrect_dist_sq(struct kdhyperrect *rect, const double *pos);  
82.   
83. #ifdef USE_LIST_NODE_ALLOCATOR  
84. static struct res_node *alloc_resnode(void);  
85. static void free_resnode(struct res_node*);  
86. #else  
87. #define alloc_resnode()     malloc(sizeof(struct res_node))  
88. #define free_resnode(n)     free(n)  
89. #endif  
90.   
91.   
92. //创建一个kdtree  
93. struct kdtree *kd_create(int k)  
94. {  
95.     struct kdtree *tree;  
96.   
97.     if(!(tree = (kdtree*)malloc(sizeof *tree))) {  
98.         return 0;  
99.     }  
100.   
101.     tree->dim = k;  
102.     tree->root = 0;  
103.     tree->destr = 0;  
104.     tree->rect = 0;  
105.   
106.     return tree;  
107. }  
108.   
109. //释放掉kdtree  
110. void kd_free(struct kdtree *tree)  
111. {  
112.     if(tree) {  
113.         kd_clear(tree);  
114.         free(tree);  
115.     }  
116. }  
117.   
118. //清除掉超平面,是按节点递归地进行的  
119. static void clear_rec(struct kdnode *node, void (*destr)(void*))  
120. {  
121.     if(!node) return;   //一个节点对应一个超平面  
122.   
123.     //递归函数，递归地清除掉二叉树左分支的超平面和二叉树右分支的超平面  
124.     clear_rec(node->left, destr);  
125.     clear_rec(node->right, destr);  
126.       
127.     //如果data清楚函数不为空,就释放掉data  
128.     if(destr)   
129.     {  
130.         destr(node->data);  
131.     }  
132.     //释放节点的坐标数组  
133.     free(node->pos);  
134.     //释放节点  
135.     free(node);  
136. }  
137.   
138. //kdtree清除  
139. void kd_clear(struct kdtree *tree)  
140. {  
141.     //清除树中每个节点的超平面,释放树中的各个节点  
142.     clear_rec(tree->root, tree->destr);  
143.     tree->root = 0;  
144.   
145.     //如果树的超平面指针不为空,对其进行释放  
146.     if (tree->rect)   
147.     {  
148.         hyperrect_free(tree->rect);  
149.         tree->rect = 0;  
150.     }  
151. }  
152.   
153. //数据销毁，用一个外来的函数来进行data的销毁  
154. void kd_data_destructor(struct kdtree *tree, void (*destr)(void*))  
155. {  
156.     //用外来的函数来执行kdtree的销毁函数  
157.     tree->destr = destr;  
158. }  
159.   
160.   
161. //在一个树节点位置处插入超矩形  
162. static int insert_rec(struct kdnode **nptr, const double *pos, void *data, int dir, int dim)  
163. {  
164.     int new_dir;  
165.     struct kdnode *node;  
166.   
167.     //如果这个节点是不存在的  
168.     if(!*nptr)   
169.     {  
170.         //分配一个结点  
171.         if(!(node = (kdnode *)malloc(sizeof *node)))   
172.         {  
173.             return -1;  
174.         }  
175.         if(!(node->pos = (double*)malloc(dim * sizeof *node->pos))) {  
176.             free(node);  
177.             return -1;  
178.         }  
179.         memcpy(node->pos, pos, dim * sizeof *node->pos);  
180.         node->data = data;  
181.         node->dir = dir;  
182.         node->left = node->right = 0;  
183.         *nptr = node;  
184.         return 0;  
185.     }  
186.   
187.     node = *nptr;  
188.     new_dir = (node->dir + 1) % dim;  
189.     if(pos[node->dir] < node->pos[node->dir]) {  
190.         return insert_rec(&(*nptr)->left, pos, data, new_dir, dim);  
191.     }  
192.     return insert_rec(&(*nptr)->right, pos, data, new_dir, dim);  
193. }  
194.   
195. //节点插入操作  
196. //参数为:要进行插入操作的kdtree,要插入的节点坐标,要插入的节点的数据  
197. int kd_insert(struct kdtree *tree, const double *pos, void *data)  
198. {  
199.     //插入超矩形  
200.     if (insert_rec(&tree->root, pos, data, 0, tree->dim))   
201.     {  
202.         return -1;  
203.     }  
204.     //如果树还没有超矩形,就创建一个超矩形  
205.     //如果已经有了超矩形,就扩展原有的超矩形  
206.     if (tree->rect == 0)   
207.     {  
208.         tree->rect = hyperrect_create(tree->dim, pos, pos);  
209.     }   
210.     else   
211.     {  
212.         hyperrect_extend(tree->rect, pos);  
213.     }  
214.   
215.     return 0;  
216. }  
217.   
218. //插入float型坐标的节点  
219. //参数为:要进行插入操作的kdtree,要插入的节点坐标,要插入的节点的数据  
220. //将float型的坐标赋值给double型的缓冲区,经过这个类型转化后进行插入  
221. //本质上是一种类型转化  
222. int kd_insertf(struct kdtree *tree, const float *pos, void *data)  
223. {  
224.     static double sbuf[16];  
225.     double *bptr, *buf = 0;  
226.     int res, dim = tree->dim;  
227.   
228.     //如果kdtree的维数大于16, 分配dim维double类型的数组  
229.     if(dim > 16)   
230.     {  
231. #ifndef NO_ALLOCA  
232.         if(dim <= 256)  
233.             bptr = buf = (double*)alloca(dim * sizeof *bptr);  
234.         else  
235. #endif  
236.             if(!(bptr = buf = (double*)malloc(dim * sizeof *bptr)))   
237.             {  
238.                 return -1;  
239.             }  
240.     }   
241.     //如果kdtree的维数小于16, 直接将指针指向已分配的内存  
242.     else   
243.     {  
244.         bptr = buf = sbuf;  
245.     }  
246.   
247.     //将要插入点的位置坐标赋值给分配的数组  
248.     while(dim-- > 0)   
249.     {  
250.         *bptr++ = *pos++;  
251.     }  
252.   
253.     //调用节点插入函数kd_insert  
254.     res = kd_insert(tree, buf, data);  
255. #ifndef NO_ALLOCA  
256.     if(tree->dim > 256)  
257. #else  
258.     if(tree->dim > 16)  
259. #endif  
260.         //释放缓存  
261.         free(buf);  
262.     return res;  
263. }  
264.   
265. //给出三维坐标值的三维kdtree插入  
266. int kd_insert3(struct kdtree *tree, double x, double y, double z, void *data)  
267. {  
268.     double buf[3];  
269.     buf[0] = x;  
270.     buf[1] = y;  
271.     buf[2] = z;  
272.     return kd_insert(tree, buf, data);  
273. }  
274.   
275. //给出三维float型坐标值的三维kdtree插入  
276. int kd_insert3f(struct kdtree *tree, float x, float y, float z, void *data)  
277. {  
278.     double buf[3];  
279.     buf[0] = x;  
280.     buf[1] = y;  
281.     buf[2] = z;  
282.     return kd_insert(tree, buf, data);  
283. }  
284.   
285. //找到最近邻的点  
286. //参数为:树节点指针, 位置坐标, 阈值, 返回结果的节点, bool型排序,维度  
287. static int find_nearest(struct kdnode *node, const double *pos, double range, struct res_node *list, int ordered, int dim)  
288. {  
289.     double dist_sq, dx;  
290.     int i, ret, added_res = 0;  
291.   
292.     if(!node) return 0;  //注意这个地方,当节点为空的时候,表明已经查找到最终的叶子结点,返回值为零  
293.   
294.     dist_sq = 0;  
295.     //计算两个节点间的平方和  
296.     for(i=0; i<dim; i++)   
297.     {  
298.         dist_sq += SQ(node->pos[i] - pos[i]);  
299.     }  
300.     //如果距离在阈值范围内,就将其插入到返回结果链表中  
301.     if(dist_sq <= SQ(range))   
302.     {         
303.         if(rlist_insert(list, node, ordered ? dist_sq : -1.0) == -1)   
304.         {  
305.             return -1;  
306.         }  
307.         added_res = 1;  
308.     }  
309.   
310.     //在这个节点的划分方向上,求两者之间的差值  
311.     dx = pos[node->dir] - node->pos[node->dir];  
312.   
313.     //根据这个差值的符号, 选择进行递归查找的分支方向  
314.     ret = find_nearest(dx <= 0.0 ? node->left : node->right, pos, range, list, ordered, dim);  
315.     //如果返回的值大于等于零,表明在这个分支中有满足条件的节点,则返回结果的个数进行累加,并在节点的另一个方向进行查找最近的节点  
316.     if(ret >= 0 && fabs(dx) < range)   
317.     {  
318.         added_res += ret;  
319.         ret = find_nearest(dx <= 0.0 ? node->right : node->left, pos, range, list, ordered, dim);  
320.     }  
321.     if(ret == -1)   
322.     {  
323.         return -1;  
324.     }  
325.     added_res += ret;  
326.   
327.     return added_res;  
328. }  
329.   
330.   
331. //找到最近邻的n个节点  
332. #if 0  
333. static int find_nearest_n(struct kdnode *node, const double *pos, double range, int num, struct rheap *heap, int dim)  
334. {  
335.     double dist_sq, dx;  
336.     int i, ret, added_res = 0;  
337.   
338.     if(!node) return 0;  
339.       
340.     /* if the photon is close enough, add it to the result heap */  
341.     //如果足够近就将其加入到结果堆中  
342.     dist_sq = 0;  
343.     //计算两者间的欧式距离  
344.     for(i=0; i<dim; i++)   
345.     {  
346.         dist_sq += SQ(node->pos[i] - pos[i]);  
347.     }  
348.     //如果计算所得距离小于阈值  
349.     if(dist_sq <= range_sq) {  
350.     //如果堆的大小大于num,也就是大于总的要找的节点数  
351.         if(heap->size >= num)  
352.         {  
353.             /* get furthest element */  
354.             //得到最远的节点  
355.             struct res_node *maxelem = rheap_get_max(heap);  
356.   
357.             /* and check if the new one is closer than that */  
358.             //测试这个节点是不是比最远的节点要近  
359.             if(maxelem->dist_sq > dist_sq)   
360.             {  
361.             //如果是的话,就移除最远的节点  
362.                 rheap_remove_max(heap);  
363.                 //并将此节点插入堆中  
364.                 if(rheap_insert(heap, node, dist_sq) == -1)   
365.                 {  
366.                     return -1;  
367.                 }  
368.                 added_res = 1;  
369.   
370.                 range_sq = dist_sq;  
371.             }  
372.         }   
373.         //如果堆的大小小于num,直接将此节点插入堆中  
374.         else   
375.         {  
376.             if(rheap_insert(heap, node, dist_sq) == -1)   
377.             {  
378.                 return =1;  
379.             }  
380.             added_res = 1;  
381.         }  
382.     }  
383.   
384.   
385.     /* find signed distance from the splitting plane */  
386.     dx = pos[node->dir] - node->pos[node->dir];  
387.   
388.     ret = find_nearest_n(dx <= 0.0 ? node->left : node->right, pos, range, num, heap, dim);  
389.     if(ret >= 0 && fabs(dx) < range) {  
390.         added_res += ret;  
391.         ret = find_nearest_n(dx <= 0.0 ? node->right : node->left, pos, range, num, heap, dim);  
392.     }  
393. }  
394. #endif  
395.   
396.   
397. static void kd_nearest_i(struct kdnode *node, const double *pos, struct kdnode **result, double *result_dist_sq, struct kdhyperrect* rect)  
398. {  
399.     int dir = node->dir;  
400.     int i;  
401.     double dummy, dist_sq;  
402.     struct kdnode *nearer_subtree, *farther_subtree;  
403.     double *nearer_hyperrect_coord, *farther_hyperrect_coord;  
404.   
405.     /* Decide whether to go left or right in the tree */  
406.     //在二叉树中,决定向左走还是向右走  
407.     dummy = pos[dir] - node->pos[dir];  
408.     if (dummy <= 0)   
409.     {  
410.         nearer_subtree = node->left;  
411.         farther_subtree = node->right;  
412.         nearer_hyperrect_coord = rect->max + dir;  
413.         farther_hyperrect_coord = rect->min + dir;  
414.     }   
415.     else   
416.     {  
417.         nearer_subtree = node->right;  
418.         farther_subtree = node->left;  
419.         nearer_hyperrect_coord = rect->min + dir;  
420.         farther_hyperrect_coord = rect->max + dir;  
421.     }  
422.   
423.     if (nearer_subtree) {  
424.         /* Slice the hyperrect to get the hyperrect of the nearer subtree */  
425.         dummy = *nearer_hyperrect_coord;  
426.         *nearer_hyperrect_coord = node->pos[dir];  
427.         /* Recurse down into nearer subtree */  
428.         kd_nearest_i(nearer_subtree, pos, result, result_dist_sq, rect);  
429.         /* Undo the slice */  
430.         *nearer_hyperrect_coord = dummy;  
431.     }  
432.   
433.     /* Check the distance of the point at the current node, compare it 
434.      * with our best so far */  
435.     dist_sq = 0;  
436.     for(i=0; i < rect->dim; i++)   
437.     {  
438.         dist_sq += SQ(node->pos[i] - pos[i]);  
439.     }  
440.     if (dist_sq < *result_dist_sq)   
441.     {  
442.         *result = node;  
443.         *result_dist_sq = dist_sq;  
444.     }  
445.   
446.     if (farther_subtree) {  
447.         /* Get the hyperrect of the farther subtree */  
448.         dummy = *farther_hyperrect_coord;  
449.         *farther_hyperrect_coord = node->pos[dir];  
450.         /* Check if we have to recurse down by calculating the closest 
451.          * point of the hyperrect and see if it's closer than our 
452.          * minimum distance in result_dist_sq. */  
453.         if (hyperrect_dist_sq(rect, pos) < *result_dist_sq) {  
454.             /* Recurse down into farther subtree */  
455.             kd_nearest_i(farther_subtree, pos, result, result_dist_sq, rect);  
456.         }  
457.         /* Undo the slice on the hyperrect */  
458.         *farther_hyperrect_coord = dummy;  
459.     }  
460. }  
461.   
462. //求kdtree中与点pos最近邻的值  
463. struct kdres *kd_nearest(struct kdtree *kd, const double *pos)  
464. {  
465.     struct kdhyperrect *rect;  
466.     struct kdnode *result;  
467.     struct kdres *rset;  
468.     double dist_sq;  
469.     int i;  
470.   
471.     //如果kd不存在,或者其超平面不存在的话,则就不会有结果  
472.     if (!kd) return 0;  
473.     if (!kd->rect) return 0;  
474.   
475.     /* Allocate result set */  
476.     //为返回结果集合分配空间  
477.     if(!(rset = (kdres*)malloc(sizeof *rset)))   
478.     {  
479.         return 0;  
480.     }  
481.     if(!(rset->rlist = (res_node*)alloc_resnode())) {  
482.         free(rset);  
483.         return 0;  
484.     }  
485.     rset->rlist->next = 0;  
486.     rset->tree = kd;  
487.   
488.     /* Duplicate the bounding hyperrectangle, we will work on the copy */  
489.     //复制边界超平面  
490.     if (!(rect = hyperrect_duplicate(kd->rect)))   
491.     {  
492.         kd_res_free(rset);  
493.         return 0;  
494.     }  
495.   
496.     /* Our first guesstimate is the root node */  
497.     result = kd->root;  
498.     dist_sq = 0;  
499.     for (i = 0; i < kd->dim; i++)  
500.         dist_sq += SQ(result->pos[i] - pos[i]);  
501.   
502.     /* Search for the nearest neighbour recursively */  
503.     //递归地查找最近邻的邻居  
504.     kd_nearest_i(kd->root, pos, &result, &dist_sq, rect);  
505.   
506.     /* Free the copy of the hyperrect */  
507.     //释放超矩形  
508.     hyperrect_free(rect);  
509.   
510.     /* Store the result */  
511.     //存储结果  
512.     if (result)   
513.     {  
514.         if (rlist_insert(rset->rlist, result, -1.0) == -1)   
515.         {  
516.             kd_res_free(rset);  
517.             return 0;  
518.         }  
519.         rset->size = 1;  
520.         kd_res_rewind(rset);  
521.         return rset;  
522.     }   
523.     else   
524.     {  
525.         kd_res_free(rset);  
526.         return 0;  
527.     }  
528. }  
529.   
530. //kd_nearest的float特例  
531. struct kdres *kd_nearestf(struct kdtree *tree, const float *pos)  
532. {  
533.     static double sbuf[16];  
534.     double *bptr, *buf = 0;  
535.     int dim = tree->dim;  
536.     struct kdres *res;  
537.   
538.     if(dim > 16) {  
539. #ifndef NO_ALLOCA  
540.         if(dim <= 256)  
541.             bptr = buf = (double*)alloca(dim * sizeof *bptr);  
542.         else  
543. #endif  
544.             if(!(bptr = buf = (double*)malloc(dim * sizeof *bptr))) {  
545.                 return 0;  
546.             }  
547.     } else {  
548.         bptr = buf = sbuf;  
549.     }  
550.   
551.     while(dim-- > 0) {  
552.         *bptr++ = *pos++;  
553.     }  
554.   
555.     res = kd_nearest(tree, buf);  
556. #ifndef NO_ALLOCA  
557.     if(tree->dim > 256)  
558. #else  
559.     if(tree->dim > 16)  
560. #endif  
561.         free(buf);  
562.     return res;  
563. }  
564.   
565. //kd_nearest的三坐标特例  
566. struct kdres *kd_nearest3(struct kdtree *tree, double x, double y, double z)  
567. {  
568.     double pos[3];  
569.     pos[0] = x;  
570.     pos[1] = y;  
571.     pos[2] = z;  
572.     return kd_nearest(tree, pos);  
573. }  
574.   
575. //kd_nearest的三坐标float特例  
576. struct kdres *kd_nearest3f(struct kdtree *tree, float x, float y, float z)  
577. {  
578.     double pos[3];  
579.     pos[0] = x;  
580.     pos[1] = y;  
581.     pos[2] = z;  
582.     return kd_nearest(tree, pos);  
583. }  
584.   
585. /* ---- nearest N search ---- */  
586. /* 
587. static kdres *kd_nearest_n(struct kdtree *kd, const double *pos, int num) 
588. { 
589.     int ret; 
590.     struct kdres *rset; 
591.  
592.     if(!(rset = malloc(sizeof *rset))) { 
593.         return 0; 
594.     } 
595.     if(!(rset->rlist = alloc_resnode())) { 
596.         free(rset); 
597.         return 0; 
598.     } 
599.     rset->rlist->next = 0; 
600.     rset->tree = kd; 
601.  
602.     if((ret = find_nearest_n(kd->root, pos, range, num, rset->rlist, kd->dim)) == -1) { 
603.         kd_res_free(rset); 
604.         return 0; 
605.     } 
606.     rset->size = ret; 
607.     kd_res_rewind(rset); 
608.     return rset; 
609. }*/  
610.   
611. //找到满足距离小于range值的节点  
612. struct kdres *kd_nearest_range(struct kdtree *kd, const double *pos, double range)  
613. {  
614.     int ret;  
615.     struct kdres *rset;  
616.   
617.     if(!(rset = (kdres*)malloc(sizeof *rset))) {  
618.         return 0;  
619.     }  
620.     if(!(rset->rlist = (res_node*)alloc_resnode())) {  
621.         free(rset);  
622.         return 0;  
623.     }  
624.     rset->rlist->next = 0;  
625.     rset->tree = kd;  
626.   
627.     if((ret = find_nearest(kd->root, pos, range, rset->rlist, 0, kd->dim)) == -1) {  
628.         kd_res_free(rset);  
629.         return 0;  
630.     }  
631.     rset->size = ret;  
632.     kd_res_rewind(rset);  
633.     return rset;  
634. }  
635.   
636. //kd_nearest_range的float特例  
637. struct kdres *kd_nearest_rangef(struct kdtree *kd, const float *pos, float range)  
638. {  
639.     static double sbuf[16];  
640.     double *bptr, *buf = 0;  
641.     int dim = kd->dim;  
642.     struct kdres *res;  
643.   
644.     if(dim > 16) {  
645. #ifndef NO_ALLOCA  
646.         if(dim <= 256)  
647.             bptr = buf = (double*)alloca(dim * sizeof *bptr);  
648.         else  
649. #endif  
650.             if(!(bptr = buf = (double*)malloc(dim * sizeof *bptr))) {  
651.                 return 0;  
652.             }  
653.     } else {  
654.         bptr = buf = sbuf;  
655.     }  
656.   
657.     while(dim-- > 0) {  
658.         *bptr++ = *pos++;  
659.     }  
660.   
661.     res = kd_nearest_range(kd, buf, range);  
662. #ifndef NO_ALLOCA  
663.     if(kd->dim > 256)  
664. #else  
665.     if(kd->dim > 16)  
666. #endif  
667.         free(buf);  
668.     return res;  
669. }  
670.   
671. //kd_nearest_range的三坐标特例  
672. struct kdres *kd_nearest_range3(struct kdtree *tree, double x, double y, double z, double range)  
673. {  
674.     double buf[3];  
675.     buf[0] = x;  
676.     buf[1] = y;  
677.     buf[2] = z;  
678.     return kd_nearest_range(tree, buf, range);  
679. }  
680.   
681. //kd_nearest_range的三坐标float特例  
682. struct kdres *kd_nearest_range3f(struct kdtree *tree, float x, float y, float z, float range)  
683. {  
684.     double buf[3];  
685.     buf[0] = x;  
686.     buf[1] = y;  
687.     buf[2] = z;  
688.     return kd_nearest_range(tree, buf, range);  
689. }  
690.   
691. //返回结果的释放  
692. void kd_res_free(struct kdres *rset)  
693. {  
694.     clear_results(rset);  
695.     free_resnode(rset->rlist);  
696.     free(rset);  
697. }  
698.   
699. //获取返回结果集合的大小  
700. int kd_res_size(struct kdres *set)  
701. {  
702.     return (set->size);  
703. }  
704.   
705. //再次回到这个节点本身的位置  
706. void kd_res_rewind(struct kdres *rset)  
707. {  
708.     rset->riter = rset->rlist->next;  
709. }  
710.   
711. //找到返回结果中的最终节点  
712. int kd_res_end(struct kdres *rset)  
713. {  
714.     return rset->riter == 0;  
715. }  
716.   
717. //返回结果列表中的下一个节点  
718. int kd_res_next(struct kdres *rset)  
719. {  
720.     rset->riter = rset->riter->next;  
721.     return rset->riter != 0;  
722. }  
723.   
724. //将返回结果的节点的坐标和data抽取出来  
725. void *kd_res_item(struct kdres *rset, double *pos)  
726. {  
727.     if(rset->riter) {  
728.         if(pos) {  
729.             memcpy(pos, rset->riter->item->pos, rset->tree->dim * sizeof *pos);  
730.         }  
731.         return rset->riter->item->data;  
732.     }  
733.     return 0;  
734. }  
735.   
736. //将返回结果的节点的坐标和data抽取出来,坐标为float型的值  
737. void *kd_res_itemf(struct kdres *rset, float *pos)  
738. {  
739.     if(rset->riter) {  
740.         if(pos) {  
741.             int i;  
742.             for(i=0; i<rset->tree->dim; i++) {  
743.                 pos[i] = rset->riter->item->pos[i];  
744.             }  
745.         }  
746.         return rset->riter->item->data;  
747.     }  
748.     return 0;  
749. }  
750.   
751. //将返回结果的节点的坐标和data抽取出来,坐标具体形式给出  
752. void *kd_res_item3(struct kdres *rset, double *x, double *y, double *z)  
753. {  
754.     if(rset->riter) {  
755.         if(*x) *x = rset->riter->item->pos[0];  
756.         if(*y) *y = rset->riter->item->pos[1];  
757.         if(*z) *z = rset->riter->item->pos[2];  
758.     }  
759.     return 0;  
760. }  
761.   
762. //将返回结果的节点的坐标和data抽取出来,坐标为float型的值,坐标具体形式给出  
763. void *kd_res_item3f(struct kdres *rset, float *x, float *y, float *z)  
764. {  
765.     if(rset->riter) {  
766.         if(*x) *x = rset->riter->item->pos[0];  
767.         if(*y) *y = rset->riter->item->pos[1];  
768.         if(*z) *z = rset->riter->item->pos[2];  
769.     }  
770.     return 0;  
771. }  
772.   
773. //获取data数据  
774. void *kd_res_item_data(struct kdres *set)  
775. {  
776.     return kd_res_item(set, 0);  
777. }  
778.   
779. /* ---- hyperrectangle helpers ---- */  
780. //创建超平面,包括三个参数:维度,每维的最小值和最大值数组  
781. static struct kdhyperrect* hyperrect_create(int dim, const double *min, const double *max)  
782. {  
783.     size_t size = dim * sizeof(double);  
784.     struct kdhyperrect* rect = 0;  
785.   
786.     if (!(rect = (kdhyperrect*)malloc(sizeof(struct kdhyperrect))))   
787.     {  
788.         return 0;  
789.     }  
790.   
791.     rect->dim = dim;  
792.     if (!(rect->min = (double*)malloc(size))) {  
793.         free(rect);  
794.         return 0;  
795.     }  
796.     if (!(rect->max = (double*)malloc(size))) {  
797.         free(rect->min);  
798.         free(rect);  
799.         return 0;  
800.     }  
801.     memcpy(rect->min, min, size);  
802.     memcpy(rect->max, max, size);  
803.   
804.     return rect;  
805. }  
806.   
807. //释放超平面结构体  
808. static void hyperrect_free(struct kdhyperrect *rect)  
809. {  
810.     free(rect->min);  
811.     free(rect->max);  
812.     free(rect);  
813. }  
814.   
815. //赋值超平面结构体  
816. static struct kdhyperrect* hyperrect_duplicate(const struct kdhyperrect *rect)  
817. {  
818.     return hyperrect_create(rect->dim, rect->min, rect->max);  
819. }  
820.   
821. //更新超平面结构体最大\最小值数组  
822. static void hyperrect_extend(struct kdhyperrect *rect, const double *pos)  
823. {  
824.     int i;  
825.   
826.     for (i=0; i < rect->dim; i++) {  
827.         if (pos[i] < rect->min[i]) {  
828.             rect->min[i] = pos[i];  
829.         }  
830.         if (pos[i] > rect->max[i]) {  
831.             rect->max[i] = pos[i];  
832.         }  
833.     }  
834. }  
835.   
836. //计算固定坐标点与超平面之间的距离  
837. static double hyperrect_dist_sq(struct kdhyperrect *rect, const double *pos)  
838. {  
839.     int i;  
840.     double result = 0;  
841.   
842.     for (i=0; i < rect->dim; i++)   
843.     {  
844.         if (pos[i] < rect->min[i])   
845.         {  
846.             result += SQ(rect->min[i] - pos[i]);  
847.         }   
848.         else if (pos[i] > rect->max[i])   
849.         {  
850.             result += SQ(rect->max[i] - pos[i]);  
851.         }  
852.     }  
853.     return result;  
854. }  
855.   
856.   
857. /* ---- static helpers ---- */  
858. #ifdef USE_LIST_NODE_ALLOCATOR  
859. /* special list node allocators. */  
860. static struct res_node *free_nodes;  
861.   
862. #ifndef NO_PTHREADS  
863. static pthread_mutex_t alloc_mutex = PTHREAD_MUTEX_INITIALIZER;  
864. #endif  
865.   
866. //创建返回结果节点  
867. static struct res_node *alloc_resnode(void)  
868. {  
869.     struct res_node *node;  
870.   
871. #ifndef NO_PTHREADS  
872.     pthread_mutex_lock(&alloc_mutex);  
873. #endif  
874.   
875.     if(!free_nodes) {  
876.         node = malloc(sizeof *node);  
877.     } else {  
878.         node = free_nodes;  
879.         free_nodes = free_nodes->next;  
880.         node->next = 0;  
881.     }  
882.   
883. #ifndef NO_PTHREADS  
884.     pthread_mutex_unlock(&alloc_mutex);  
885. #endif  
886.   
887.     return node;  
888. }  
889.   
890. //释放返回结果节点  
891. static void free_resnode(struct res_node *node)  
892. {  
893. #ifndef NO_PTHREADS  
894.     pthread_mutex_lock(&alloc_mutex);  
895. #endif  
896.   
897.     node->next = free_nodes;  
898.     free_nodes = node;  
899.   
900. #ifndef NO_PTHREADS  
901.     pthread_mutex_unlock(&alloc_mutex);  
902. #endif  
903. }  
904. #endif  /* list node allocator or not */  
905.   
906.   
907. /* inserts the item. if dist_sq is >= 0, then do an ordered insert */  
908. /* TODO make the ordering code use heapsort */  
909. //函数参数: 返回结果节点指针,树节点指针,距离函数  
910. //将一个结果节点插入到返回结果的列表中  
911. static int rlist_insert(struct res_node *list, struct kdnode *item, double dist_sq)  
912. {  
913.     struct res_node *rnode;  
914.   
915.     //创建一个返回结果的节点  
916.     if(!(rnode = (res_node*)alloc_resnode()))   
917.     {  
918.         return -1;  
919.     }  
920.     rnode->item = item;           //对应的树节点  
921.     rnode->dist_sq = dist_sq;     //对应的距离值  
922.   
923.     //当距离大于零的时候  
924.     if(dist_sq >= 0.0)   
925.     {  
926.         while(list->next && list->next->dist_sq < dist_sq)   
927.         {  
928.             list = list->next;  
929.         }  
930.     }  
931.     rnode->next = list->next;  
932.     list->next = rnode;  
933.     return 0;  
934. }  
935.   
936. //清除返回结果的集合  
937. //本质上是个双链表中单链表的清理  
938. static void clear_results(struct kdres *rset)  
939. {  
940.     struct res_node *tmp, *node = rset->rlist->next;  
941.   
942.     while(node)   
943.     {  
944.         tmp = node;  
945.         node = node->next;  
946.         free_resnode(tmp);  
947.     }  
948.   
949.     rset->rlist->next = 0;  
950. }