- kdtree的原理就是基於二叉樹的形式,將高維空間用超矩形進行劃分.其主要用途是用來求解高維空間中最近鄰的值.
kdtree的原理就是基於二叉樹的形式,將高維空間用超矩形進行劃分.其主要用途是用來求解高維空間中最近鄰的值.
下面是kdtree.h文件,是kdtree數據結構的頭文件
- #ifndef _KDTREE_H_
- #define _KDTREE_H_
- #ifdef __cplusplus
- extern "C" {
- #endif
- struct kdtree;
- struct kdres;
- /* create a kd-tree for "k"-dimensional data */
- struct kdtree *kd_create(int k);
- /* free the struct kdtree */
- void kd_free(struct kdtree *tree);
- /* remove all the elements from the tree */
- void kd_clear(struct kdtree *tree);
- /* if called with non-null 2nd argument, the function provided
- * will be called on data pointers (see kd_insert) when nodes
- * are to be removed from the tree.
- */
- void kd_data_destructor(struct kdtree *tree, void (*destr)(void*));
- /* insert a node, specifying its position, and optional data */
- int kd_insert(struct kdtree *tree, const double *pos, void *data);
- int kd_insertf(struct kdtree *tree, const float *pos, void *data);
- int kd_insert3(struct kdtree *tree, double x, double y, double z, void *data);
- int kd_insert3f(struct kdtree *tree, float x, float y, float z, void *data);
- /* Find the nearest node from a given point.
- *
- * This function returns a pointer to a result set with at most one element.
- */
- struct kdres *kd_nearest(struct kdtree *tree, const double *pos);
- struct kdres *kd_nearestf(struct kdtree *tree, const float *pos);
- struct kdres *kd_nearest3(struct kdtree *tree, double x, double y, double z);
- struct kdres *kd_nearest3f(struct kdtree *tree, float x, float y, float z);
- /* Find the N nearest nodes from a given point.
- *
- * This function returns a pointer to a result set, with at most N elements,
- * which can be manipulated with the kd_res_* functions.
- * The returned pointer can be null as an indication of an error. Otherwise
- * a valid result set is always returned which may contain 0 or more elements.
- * The result set must be deallocated with kd_res_free after use.
- */
- /*
- struct kdres *kd_nearest_n(struct kdtree *tree, const double *pos, int num);
- struct kdres *kd_nearest_nf(struct kdtree *tree, const float *pos, int num);
- struct kdres *kd_nearest_n3(struct kdtree *tree, double x, double y, double z);
- struct kdres *kd_nearest_n3f(struct kdtree *tree, float x, float y, float z);
- */
- /* Find any nearest nodes from a given point within a range.
- *
- * This function returns a pointer to a result set, which can be manipulated
- * by the kd_res_* functions.
- * The returned pointer can be null as an indication of an error. Otherwise
- * a valid result set is always returned which may contain 0 or more elements.
- * The result set must be deallocated with kd_res_free after use.
- */
- struct kdres *kd_nearest_range(struct kdtree *tree, const double *pos, double range);
- struct kdres *kd_nearest_rangef(struct kdtree *tree, const float *pos, float range);
- struct kdres *kd_nearest_range3(struct kdtree *tree, double x, double y, double z, double range);
- struct kdres *kd_nearest_range3f(struct kdtree *tree, float x, float y, float z, float range);
- /* frees a result set returned by kd_nearest_range() */
- void kd_res_free(struct kdres *set);
- /* returns the size of the result set (in elements) */
- int kd_res_size(struct kdres *set);
- /* rewinds the result set iterator */
- void kd_res_rewind(struct kdres *set);
- /* returns non-zero if the set iterator reached the end after the last element */
- int kd_res_end(struct kdres *set);
- /* advances the result set iterator, returns non-zero on success, zero if
- * there are no more elements in the result set.
- */
- int kd_res_next(struct kdres *set);
- /* returns the data pointer (can be null) of the current result set item
- * and optionally sets its position to the pointers(s) if not null.
- */
- void *kd_res_item(struct kdres *set, double *pos);
- void *kd_res_itemf(struct kdres *set, float *pos);
- void *kd_res_item3(struct kdres *set, double *x, double *y, double *z);
- void *kd_res_item3f(struct kdres *set, float *x, float *y, float *z);
- /* equivalent to kd_res_item(set, 0) */
- void *kd_res_item_data(struct kdres *set);
- #ifdef __cplusplus
- }
- #endif
- #endif /* _KDTREE_H_ */
#ifndef _KDTREE_H_
#define _KDTREE_H_
#ifdef __cplusplus
extern "C" {
#endif
struct kdtree;
struct kdres;
/* create a kd-tree for "k"-dimensional data */
struct kdtree *kd_create(int k);
/* free the struct kdtree */
void kd_free(struct kdtree *tree);
/* remove all the elements from the tree */
void kd_clear(struct kdtree *tree);
/* if called with non-null 2nd argument, the function provided
* will be called on data pointers (see kd_insert) when nodes
* are to be removed from the tree.
*/
void kd_data_destructor(struct kdtree *tree, void (*destr)(void*));
/* insert a node, specifying its position, and optional data */
int kd_insert(struct kdtree *tree, const double *pos, void *data);
int kd_insertf(struct kdtree *tree, const float *pos, void *data);
int kd_insert3(struct kdtree *tree, double x, double y, double z, void *data);
int kd_insert3f(struct kdtree *tree, float x, float y, float z, void *data);
/* Find the nearest node from a given point.
*
* This function returns a pointer to a result set with at most one element.
*/
struct kdres *kd_nearest(struct kdtree *tree, const double *pos);
struct kdres *kd_nearestf(struct kdtree *tree, const float *pos);
struct kdres *kd_nearest3(struct kdtree *tree, double x, double y, double z);
struct kdres *kd_nearest3f(struct kdtree *tree, float x, float y, float z);
/* Find the N nearest nodes from a given point.
*
* This function returns a pointer to a result set, with at most N elements,
* which can be manipulated with the kd_res_* functions.
* The returned pointer can be null as an indication of an error. Otherwise
* a valid result set is always returned which may contain 0 or more elements.
* The result set must be deallocated with kd_res_free after use.
*/
/*
struct kdres *kd_nearest_n(struct kdtree *tree, const double *pos, int num);
struct kdres *kd_nearest_nf(struct kdtree *tree, const float *pos, int num);
struct kdres *kd_nearest_n3(struct kdtree *tree, double x, double y, double z);
struct kdres *kd_nearest_n3f(struct kdtree *tree, float x, float y, float z);
*/
/* Find any nearest nodes from a given point within a range.
*
* This function returns a pointer to a result set, which can be manipulated
* by the kd_res_* functions.
* The returned pointer can be null as an indication of an error. Otherwise
* a valid result set is always returned which may contain 0 or more elements.
* The result set must be deallocated with kd_res_free after use.
*/
struct kdres *kd_nearest_range(struct kdtree *tree, const double *pos, double range);
struct kdres *kd_nearest_rangef(struct kdtree *tree, const float *pos, float range);
struct kdres *kd_nearest_range3(struct kdtree *tree, double x, double y, double z, double range);
struct kdres *kd_nearest_range3f(struct kdtree *tree, float x, float y, float z, float range);
/* frees a result set returned by kd_nearest_range() */
void kd_res_free(struct kdres *set);
/* returns the size of the result set (in elements) */
int kd_res_size(struct kdres *set);
/* rewinds the result set iterator */
void kd_res_rewind(struct kdres *set);
/* returns non-zero if the set iterator reached the end after the last element */
int kd_res_end(struct kdres *set);
/* advances the result set iterator, returns non-zero on success, zero if
* there are no more elements in the result set.
*/
int kd_res_next(struct kdres *set);
/* returns the data pointer (can be null) of the current result set item
* and optionally sets its position to the pointers(s) if not null.
*/
void *kd_res_item(struct kdres *set, double *pos);
void *kd_res_itemf(struct kdres *set, float *pos);
void *kd_res_item3(struct kdres *set, double *x, double *y, double *z);
void *kd_res_item3f(struct kdres *set, float *x, float *y, float *z);
/* equivalent to kd_res_item(set, 0) */
void *kd_res_item_data(struct kdres *set);
#ifdef __cplusplus
}
#endif
#endif /* _KDTREE_H_ */
下面是kdtree.c 文件,上面有我自己精心整理的詳細的漢語註釋,這個版本的kdtree可直接拿來用
- //kd_tree.h kd_tree的頭文件
- #include "stdafx.h"
- //頭文件
- #include <stdio.h>
- #include <stdlib.h>
- #include <string.h>
- #include <math.h>
- #include "kd_tree.h"
- #if defined(WIN32) || defined(__WIN32__)
- #include <malloc.h>
- #endif
- #ifdef USE_LIST_NODE_ALLOCATOR
- #ifndef NO_PTHREADS
- #include <pthread.h>
- #else
- #ifndef I_WANT_THREAD_BUGS
- #error "You are compiling with the fast list node allocator, with pthreads disabled! This WILL break if used from multiple threads."
- #endif /* I want thread bugs */
- #endif /* pthread support */
- #endif /* use list node allocator */
- //超平面的結構體
- //包括一個屬性的維數和每維座標的最大和最小值構成的數組
- struct kdhyperrect {
- int dim;
- double *min, *max; /* minimum/maximum coords */
- };
- //節點的結構體,也就是事例的結構體
- struct kdnode {
- double *pos;
- int dir;
- void *data;
- struct kdnode *left, *right; /* negative/positive side */
- };
- //返回結果節點, 包括樹的節點,距離值, 是一個單鏈表的形式
- struct res_node {
- struct kdnode *item;
- double dist_sq;
- struct res_node *next;
- };
- //樹有幾個屬性,一是維數,一是樹根節點,一是超平面,一是銷燬data的函數
- struct kdtree {
- int dim;
- struct kdnode *root;
- struct kdhyperrect *rect;
- void (*destr)(void*);
- };
- //kdtree的返回結果,包括kdtree,這是一個雙鏈表的形式
- struct kdres {
- struct kdtree *tree;
- struct res_node *rlist, *riter; //雙鏈表?
- int size;
- };
- //計算平方的宏定義,相當於函數
- #define SQ(x) ((x) * (x))
- static void clear_rec(struct kdnode *node, void (*destr)(void*));
- static int insert_rec(struct kdnode **node, const double *pos, void *data, int dir, int dim);
- static int rlist_insert(struct res_node *list, struct kdnode *item, double dist_sq);
- static void clear_results(struct kdres *set);
- static struct kdhyperrect* hyperrect_create(int dim, const double *min, const double *max);
- static void hyperrect_free(struct kdhyperrect *rect);
- static struct kdhyperrect* hyperrect_duplicate(const struct kdhyperrect *rect);
- static void hyperrect_extend(struct kdhyperrect *rect, const double *pos);
- static double hyperrect_dist_sq(struct kdhyperrect *rect, const double *pos);
- #ifdef USE_LIST_NODE_ALLOCATOR
- static struct res_node *alloc_resnode(void);
- static void free_resnode(struct res_node*);
- #else
- #define alloc_resnode() malloc(sizeof(struct res_node))
- #define free_resnode(n) free(n)
- #endif
- //創建一個kdtree
- struct kdtree *kd_create(int k)
- {
- struct kdtree *tree;
- if(!(tree = (kdtree*)malloc(sizeof *tree))) {
- return 0;
- }
- tree->dim = k;
- tree->root = 0;
- tree->destr = 0;
- tree->rect = 0;
- return tree;
- }
- //釋放掉kdtree
- void kd_free(struct kdtree *tree)
- {
- if(tree) {
- kd_clear(tree);
- free(tree);
- }
- }
- //清除掉超平面,是按節點遞歸地進行的
- static void clear_rec(struct kdnode *node, void (*destr)(void*))
- {
- if(!node) return; //一個節點對應一個超平面
- //遞歸函數,遞歸地清除掉二叉樹左分支的超平面和二叉樹右分支的超平面
- clear_rec(node->left, destr);
- clear_rec(node->right, destr);
- //如果data清楚函數不爲空,就釋放掉data
- if(destr)
- {
- destr(node->data);
- }
- //釋放節點的座標數組
- free(node->pos);
- //釋放節點
- free(node);
- }
- //kdtree清除
- void kd_clear(struct kdtree *tree)
- {
- //清除樹中每個節點的超平面,釋放樹中的各個節點
- clear_rec(tree->root, tree->destr);
- tree->root = 0;
- //如果樹的超平面指針不爲空,對其進行釋放
- if (tree->rect)
- {
- hyperrect_free(tree->rect);
- tree->rect = 0;
- }
- }
- //數據銷燬,用一個外來的函數來進行data的銷燬
- void kd_data_destructor(struct kdtree *tree, void (*destr)(void*))
- {
- //用外來的函數來執行kdtree的銷燬函數
- tree->destr = destr;
- }
- //在一個樹節點位置處插入超矩形
- static int insert_rec(struct kdnode **nptr, const double *pos, void *data, int dir, int dim)
- {
- int new_dir;
- struct kdnode *node;
- //如果這個節點是不存在的
- if(!*nptr)
- {
- //分配一個結點
- if(!(node = (kdnode *)malloc(sizeof *node)))
- {
- return -1;
- }
- if(!(node->pos = (double*)malloc(dim * sizeof *node->pos))) {
- free(node);
- return -1;
- }
- memcpy(node->pos, pos, dim * sizeof *node->pos);
- node->data = data;
- node->dir = dir;
- node->left = node->right = 0;
- *nptr = node;
- return 0;
- }
- node = *nptr;
- new_dir = (node->dir + 1) % dim;
- if(pos[node->dir] < node->pos[node->dir]) {
- return insert_rec(&(*nptr)->left, pos, data, new_dir, dim);
- }
- return insert_rec(&(*nptr)->right, pos, data, new_dir, dim);
- }
- //節點插入操作
- //參數爲:要進行插入操作的kdtree,要插入的節點座標,要插入的節點的數據
- int kd_insert(struct kdtree *tree, const double *pos, void *data)
- {
- //插入超矩形
- if (insert_rec(&tree->root, pos, data, 0, tree->dim))
- {
- return -1;
- }
- //如果樹還沒有超矩形,就創建一個超矩形
- //如果已經有了超矩形,就擴展原有的超矩形
- if (tree->rect == 0)
- {
- tree->rect = hyperrect_create(tree->dim, pos, pos);
- }
- else
- {
- hyperrect_extend(tree->rect, pos);
- }
- return 0;
- }
- //插入float型座標的節點
- //參數爲:要進行插入操作的kdtree,要插入的節點座標,要插入的節點的數據
- //將float型的座標賦值給double型的緩衝區,經過這個類型轉化後進行插入
- //本質上是一種類型轉化
- int kd_insertf(struct kdtree *tree, const float *pos, void *data)
- {
- static double sbuf[16];
- double *bptr, *buf = 0;
- int res, dim = tree->dim;
- //如果kdtree的維數大於16, 分配dim維double類型的數組
- if(dim > 16)
- {
- #ifndef NO_ALLOCA
- if(dim <= 256)
- bptr = buf = (double*)alloca(dim * sizeof *bptr);
- else
- #endif
- if(!(bptr = buf = (double*)malloc(dim * sizeof *bptr)))
- {
- return -1;
- }
- }
- //如果kdtree的維數小於16, 直接將指針指向已分配的內存
- else
- {
- bptr = buf = sbuf;
- }
- //將要插入點的位置座標賦值給分配的數組
- while(dim-- > 0)
- {
- *bptr++ = *pos++;
- }
- //調用節點插入函數kd_insert
- res = kd_insert(tree, buf, data);
- #ifndef NO_ALLOCA
- if(tree->dim > 256)
- #else
- if(tree->dim > 16)
- #endif
- //釋放緩存
- free(buf);
- return res;
- }
- //給出三維座標值的三維kdtree插入
- int kd_insert3(struct kdtree *tree, double x, double y, double z, void *data)
- {
- double buf[3];
- buf[0] = x;
- buf[1] = y;
- buf[2] = z;
- return kd_insert(tree, buf, data);
- }
- //給出三維float型座標值的三維kdtree插入
- int kd_insert3f(struct kdtree *tree, float x, float y, float z, void *data)
- {
- double buf[3];
- buf[0] = x;
- buf[1] = y;
- buf[2] = z;
- return kd_insert(tree, buf, data);
- }
- //找到最近鄰的點
- //參數爲:樹節點指針, 位置座標, 閾值, 返回結果的節點, bool型排序,維度
- static int find_nearest(struct kdnode *node, const double *pos, double range, struct res_node *list, int ordered, int dim)
- {
- double dist_sq, dx;
- int i, ret, added_res = 0;
- if(!node) return 0; //注意這個地方,當節點爲空的時候,表明已經查找到最終的葉子結點,返回值爲零
- dist_sq = 0;
- //計算兩個節點間的平方和
- for(i=0; i<dim; i++)
- {
- dist_sq += SQ(node->pos[i] - pos[i]);
- }
- //如果距離在閾值範圍內,就將其插入到返回結果鏈表中
- if(dist_sq <= SQ(range))
- {
- if(rlist_insert(list, node, ordered ? dist_sq : -1.0) == -1)
- {
- return -1;
- }
- added_res = 1;
- }
- //在這個節點的劃分方向上,求兩者之間的差值
- dx = pos[node->dir] - node->pos[node->dir];
- //根據這個差值的符號, 選擇進行遞歸查找的分支方向
- ret = find_nearest(dx <= 0.0 ? node->left : node->right, pos, range, list, ordered, dim);
- //如果返回的值大於等於零,表明在這個分支中有滿足條件的節點,則返回結果的個數進行累加,並在節點的另一個方向進行查找最近的節點
- if(ret >= 0 && fabs(dx) < range)
- {
- added_res += ret;
- ret = find_nearest(dx <= 0.0 ? node->right : node->left, pos, range, list, ordered, dim);
- }
- if(ret == -1)
- {
- return -1;
- }
- added_res += ret;
- return added_res;
- }
- //找到最近鄰的n個節點
- #if 0
- static int find_nearest_n(struct kdnode *node, const double *pos, double range, int num, struct rheap *heap, int dim)
- {
- double dist_sq, dx;
- int i, ret, added_res = 0;
- if(!node) return 0;
- /* if the photon is close enough, add it to the result heap */
- //如果足夠近就將其加入到結果堆中
- dist_sq = 0;
- //計算兩者間的歐式距離
- for(i=0; i<dim; i++)
- {
- dist_sq += SQ(node->pos[i] - pos[i]);
- }
- //如果計算所得距離小於閾值
- if(dist_sq <= range_sq) {
- //如果堆的大小大於num,也就是大於總的要找的節點數
- if(heap->size >= num)
- {
- /* get furthest element */
- //得到最遠的節點
- struct res_node *maxelem = rheap_get_max(heap);
- /* and check if the new one is closer than that */
- //測試這個節點是不是比最遠的節點要近
- if(maxelem->dist_sq > dist_sq)
- {
- //如果是的話,就移除最遠的節點
- rheap_remove_max(heap);
- //並將此節點插入堆中
- if(rheap_insert(heap, node, dist_sq) == -1)
- {
- return -1;
- }
- added_res = 1;
- range_sq = dist_sq;
- }
- }
- //如果堆的大小小於num,直接將此節點插入堆中
- else
- {
- if(rheap_insert(heap, node, dist_sq) == -1)
- {
- return =1;
- }
- added_res = 1;
- }
- }
- /* find signed distance from the splitting plane */
- dx = pos[node->dir] - node->pos[node->dir];
- ret = find_nearest_n(dx <= 0.0 ? node->left : node->right, pos, range, num, heap, dim);
- if(ret >= 0 && fabs(dx) < range) {
- added_res += ret;
- ret = find_nearest_n(dx <= 0.0 ? node->right : node->left, pos, range, num, heap, dim);
- }
- }
- #endif
- static void kd_nearest_i(struct kdnode *node, const double *pos, struct kdnode **result, double *result_dist_sq, struct kdhyperrect* rect)
- {
- int dir = node->dir;
- int i;
- double dummy, dist_sq;
- struct kdnode *nearer_subtree, *farther_subtree;
- double *nearer_hyperrect_coord, *farther_hyperrect_coord;
- /* Decide whether to go left or right in the tree */
- //在二叉樹中,決定向左走還是向右走
- dummy = pos[dir] - node->pos[dir];
- if (dummy <= 0)
- {
- nearer_subtree = node->left;
- farther_subtree = node->right;
- nearer_hyperrect_coord = rect->max + dir;
- farther_hyperrect_coord = rect->min + dir;
- }
- else
- {
- nearer_subtree = node->right;
- farther_subtree = node->left;
- nearer_hyperrect_coord = rect->min + dir;
- farther_hyperrect_coord = rect->max + dir;
- }
- if (nearer_subtree) {
- /* Slice the hyperrect to get the hyperrect of the nearer subtree */
- dummy = *nearer_hyperrect_coord;
- *nearer_hyperrect_coord = node->pos[dir];
- /* Recurse down into nearer subtree */
- kd_nearest_i(nearer_subtree, pos, result, result_dist_sq, rect);
- /* Undo the slice */
- *nearer_hyperrect_coord = dummy;
- }
- /* Check the distance of the point at the current node, compare it
- * with our best so far */
- dist_sq = 0;
- for(i=0; i < rect->dim; i++)
- {
- dist_sq += SQ(node->pos[i] - pos[i]);
- }
- if (dist_sq < *result_dist_sq)
- {
- *result = node;
- *result_dist_sq = dist_sq;
- }
- if (farther_subtree) {
- /* Get the hyperrect of the farther subtree */
- dummy = *farther_hyperrect_coord;
- *farther_hyperrect_coord = node->pos[dir];
- /* Check if we have to recurse down by calculating the closest
- * point of the hyperrect and see if it's closer than our
- * minimum distance in result_dist_sq. */
- if (hyperrect_dist_sq(rect, pos) < *result_dist_sq) {
- /* Recurse down into farther subtree */
- kd_nearest_i(farther_subtree, pos, result, result_dist_sq, rect);
- }
- /* Undo the slice on the hyperrect */
- *farther_hyperrect_coord = dummy;
- }
- }
- //求kdtree中與點pos最近鄰的值
- struct kdres *kd_nearest(struct kdtree *kd, const double *pos)
- {
- struct kdhyperrect *rect;
- struct kdnode *result;
- struct kdres *rset;
- double dist_sq;
- int i;
- //如果kd不存在,或者其超平面不存在的話,則就不會有結果
- if (!kd) return 0;
- if (!kd->rect) return 0;
- /* Allocate result set */
- //爲返回結果集合分配空間
- if(!(rset = (kdres*)malloc(sizeof *rset)))
- {
- return 0;
- }
- if(!(rset->rlist = (res_node*)alloc_resnode())) {
- free(rset);
- return 0;
- }
- rset->rlist->next = 0;
- rset->tree = kd;
- /* Duplicate the bounding hyperrectangle, we will work on the copy */
- //複製邊界超平面
- if (!(rect = hyperrect_duplicate(kd->rect)))
- {
- kd_res_free(rset);
- return 0;
- }
- /* Our first guesstimate is the root node */
- result = kd->root;
- dist_sq = 0;
- for (i = 0; i < kd->dim; i++)
- dist_sq += SQ(result->pos[i] - pos[i]);
- /* Search for the nearest neighbour recursively */
- //遞歸地查找最近鄰的鄰居
- kd_nearest_i(kd->root, pos, &result, &dist_sq, rect);
- /* Free the copy of the hyperrect */
- //釋放超矩形
- hyperrect_free(rect);
- /* Store the result */
- //存儲結果
- if (result)
- {
- if (rlist_insert(rset->rlist, result, -1.0) == -1)
- {
- kd_res_free(rset);
- return 0;
- }
- rset->size = 1;
- kd_res_rewind(rset);
- return rset;
- }
- else
- {
- kd_res_free(rset);
- return 0;
- }
- }
- //kd_nearest的float特例
- struct kdres *kd_nearestf(struct kdtree *tree, const float *pos)
- {
- static double sbuf[16];
- double *bptr, *buf = 0;
- int dim = tree->dim;
- struct kdres *res;
- if(dim > 16) {
- #ifndef NO_ALLOCA
- if(dim <= 256)
- bptr = buf = (double*)alloca(dim * sizeof *bptr);
- else
- #endif
- if(!(bptr = buf = (double*)malloc(dim * sizeof *bptr))) {
- return 0;
- }
- } else {
- bptr = buf = sbuf;
- }
- while(dim-- > 0) {
- *bptr++ = *pos++;
- }
- res = kd_nearest(tree, buf);
- #ifndef NO_ALLOCA
- if(tree->dim > 256)
- #else
- if(tree->dim > 16)
- #endif
- free(buf);
- return res;
- }
- //kd_nearest的三座標特例
- struct kdres *kd_nearest3(struct kdtree *tree, double x, double y, double z)
- {
- double pos[3];
- pos[0] = x;
- pos[1] = y;
- pos[2] = z;
- return kd_nearest(tree, pos);
- }
- //kd_nearest的三座標float特例
- struct kdres *kd_nearest3f(struct kdtree *tree, float x, float y, float z)
- {
- double pos[3];
- pos[0] = x;
- pos[1] = y;
- pos[2] = z;
- return kd_nearest(tree, pos);
- }
- /* ---- nearest N search ---- */
- /*
- static kdres *kd_nearest_n(struct kdtree *kd, const double *pos, int num)
- {
- int ret;
- struct kdres *rset;
- if(!(rset = malloc(sizeof *rset))) {
- return 0;
- }
- if(!(rset->rlist = alloc_resnode())) {
- free(rset);
- return 0;
- }
- rset->rlist->next = 0;
- rset->tree = kd;
- if((ret = find_nearest_n(kd->root, pos, range, num, rset->rlist, kd->dim)) == -1) {
- kd_res_free(rset);
- return 0;
- }
- rset->size = ret;
- kd_res_rewind(rset);
- return rset;
- }*/
- //找到滿足距離小於range值的節點
- struct kdres *kd_nearest_range(struct kdtree *kd, const double *pos, double range)
- {
- int ret;
- struct kdres *rset;
- if(!(rset = (kdres*)malloc(sizeof *rset))) {
- return 0;
- }
- if(!(rset->rlist = (res_node*)alloc_resnode())) {
- free(rset);
- return 0;
- }
- rset->rlist->next = 0;
- rset->tree = kd;
- if((ret = find_nearest(kd->root, pos, range, rset->rlist, 0, kd->dim)) == -1) {
- kd_res_free(rset);
- return 0;
- }
- rset->size = ret;
- kd_res_rewind(rset);
- return rset;
- }
- //kd_nearest_range的float特例
- struct kdres *kd_nearest_rangef(struct kdtree *kd, const float *pos, float range)
- {
- static double sbuf[16];
- double *bptr, *buf = 0;
- int dim = kd->dim;
- struct kdres *res;
- if(dim > 16) {
- #ifndef NO_ALLOCA
- if(dim <= 256)
- bptr = buf = (double*)alloca(dim * sizeof *bptr);
- else
- #endif
- if(!(bptr = buf = (double*)malloc(dim * sizeof *bptr))) {
- return 0;
- }
- } else {
- bptr = buf = sbuf;
- }
- while(dim-- > 0) {
- *bptr++ = *pos++;
- }
- res = kd_nearest_range(kd, buf, range);
- #ifndef NO_ALLOCA
- if(kd->dim > 256)
- #else
- if(kd->dim > 16)
- #endif
- free(buf);
- return res;
- }
- //kd_nearest_range的三座標特例
- struct kdres *kd_nearest_range3(struct kdtree *tree, double x, double y, double z, double range)
- {
- double buf[3];
- buf[0] = x;
- buf[1] = y;
- buf[2] = z;
- return kd_nearest_range(tree, buf, range);
- }
- //kd_nearest_range的三座標float特例
- struct kdres *kd_nearest_range3f(struct kdtree *tree, float x, float y, float z, float range)
- {
- double buf[3];
- buf[0] = x;
- buf[1] = y;
- buf[2] = z;
- return kd_nearest_range(tree, buf, range);
- }
- //返回結果的釋放
- void kd_res_free(struct kdres *rset)
- {
- clear_results(rset);
- free_resnode(rset->rlist);
- free(rset);
- }
- //獲取返回結果集合的大小
- int kd_res_size(struct kdres *set)
- {
- return (set->size);
- }
- //再次回到這個節點本身的位置
- void kd_res_rewind(struct kdres *rset)
- {
- rset->riter = rset->rlist->next;
- }
- //找到返回結果中的最終節點
- int kd_res_end(struct kdres *rset)
- {
- return rset->riter == 0;
- }
- //返回結果列表中的下一個節點
- int kd_res_next(struct kdres *rset)
- {
- rset->riter = rset->riter->next;
- return rset->riter != 0;
- }
- //將返回結果的節點的座標和data抽取出來
- void *kd_res_item(struct kdres *rset, double *pos)
- {
- if(rset->riter) {
- if(pos) {
- memcpy(pos, rset->riter->item->pos, rset->tree->dim * sizeof *pos);
- }
- return rset->riter->item->data;
- }
- return 0;
- }
- //將返回結果的節點的座標和data抽取出來,座標爲float型的值
- void *kd_res_itemf(struct kdres *rset, float *pos)
- {
- if(rset->riter) {
- if(pos) {
- int i;
- for(i=0; i<rset->tree->dim; i++) {
- pos[i] = rset->riter->item->pos[i];
- }
- }
- return rset->riter->item->data;
- }
- return 0;
- }
- //將返回結果的節點的座標和data抽取出來,座標具體形式給出
- void *kd_res_item3(struct kdres *rset, double *x, double *y, double *z)
- {
- if(rset->riter) {
- if(*x) *x = rset->riter->item->pos[0];
- if(*y) *y = rset->riter->item->pos[1];
- if(*z) *z = rset->riter->item->pos[2];
- }
- return 0;
- }
- //將返回結果的節點的座標和data抽取出來,座標爲float型的值,座標具體形式給出
- void *kd_res_item3f(struct kdres *rset, float *x, float *y, float *z)
- {
- if(rset->riter) {
- if(*x) *x = rset->riter->item->pos[0];
- if(*y) *y = rset->riter->item->pos[1];
- if(*z) *z = rset->riter->item->pos[2];
- }
- return 0;
- }
- //獲取data數據
- void *kd_res_item_data(struct kdres *set)
- {
- return kd_res_item(set, 0);
- }
- /* ---- hyperrectangle helpers ---- */
- //創建超平面,包括三個參數:維度,每維的最小值和最大值數組
- static struct kdhyperrect* hyperrect_create(int dim, const double *min, const double *max)
- {
- size_t size = dim * sizeof(double);
- struct kdhyperrect* rect = 0;
- if (!(rect = (kdhyperrect*)malloc(sizeof(struct kdhyperrect))))
- {
- return 0;
- }
- rect->dim = dim;
- if (!(rect->min = (double*)malloc(size))) {
- free(rect);
- return 0;
- }
- if (!(rect->max = (double*)malloc(size))) {
- free(rect->min);
- free(rect);
- return 0;
- }
- memcpy(rect->min, min, size);
- memcpy(rect->max, max, size);
- return rect;
- }
- //釋放超平面結構體
- static void hyperrect_free(struct kdhyperrect *rect)
- {
- free(rect->min);
- free(rect->max);
- free(rect);
- }
- //賦值超平面結構體
- static struct kdhyperrect* hyperrect_duplicate(const struct kdhyperrect *rect)
- {
- return hyperrect_create(rect->dim, rect->min, rect->max);
- }
- //更新超平面結構體最大\最小值數組
- static void hyperrect_extend(struct kdhyperrect *rect, const double *pos)
- {
- int i;
- for (i=0; i < rect->dim; i++) {
- if (pos[i] < rect->min[i]) {
- rect->min[i] = pos[i];
- }
- if (pos[i] > rect->max[i]) {
- rect->max[i] = pos[i];
- }
- }
- }
- //計算固定座標點與超平面之間的距離
- static double hyperrect_dist_sq(struct kdhyperrect *rect, const double *pos)
- {
- int i;
- double result = 0;
- for (i=0; i < rect->dim; i++)
- {
- if (pos[i] < rect->min[i])
- {
- result += SQ(rect->min[i] - pos[i]);
- }
- else if (pos[i] > rect->max[i])
- {
- result += SQ(rect->max[i] - pos[i]);
- }
- }
- return result;
- }
- /* ---- static helpers ---- */
- #ifdef USE_LIST_NODE_ALLOCATOR
- /* special list node allocators. */
- static struct res_node *free_nodes;
- #ifndef NO_PTHREADS
- static pthread_mutex_t alloc_mutex = PTHREAD_MUTEX_INITIALIZER;
- #endif
- //創建返回結果節點
- static struct res_node *alloc_resnode(void)
- {
- struct res_node *node;
- #ifndef NO_PTHREADS
- pthread_mutex_lock(&alloc_mutex);
- #endif
- if(!free_nodes) {
- node = malloc(sizeof *node);
- } else {
- node = free_nodes;
- free_nodes = free_nodes->next;
- node->next = 0;
- }
- #ifndef NO_PTHREADS
- pthread_mutex_unlock(&alloc_mutex);
- #endif
- return node;
- }
- //釋放返回結果節點
- static void free_resnode(struct res_node *node)
- {
- #ifndef NO_PTHREADS
- pthread_mutex_lock(&alloc_mutex);
- #endif
- node->next = free_nodes;
- free_nodes = node;
- #ifndef NO_PTHREADS
- pthread_mutex_unlock(&alloc_mutex);
- #endif
- }
- #endif /* list node allocator or not */
- /* inserts the item. if dist_sq is >= 0, then do an ordered insert */
- /* TODO make the ordering code use heapsort */
- //函數參數: 返回結果節點指針,樹節點指針,距離函數
- //將一個結果節點插入到返回結果的列表中
- static int rlist_insert(struct res_node *list, struct kdnode *item, double dist_sq)
- {
- struct res_node *rnode;
- //創建一個返回結果的節點
- if(!(rnode = (res_node*)alloc_resnode()))
- {
- return -1;
- }
- rnode->item = item; //對應的樹節點
- rnode->dist_sq = dist_sq; //對應的距離值
- //當距離大於零的時候
- if(dist_sq >= 0.0)
- {
- while(list->next && list->next->dist_sq < dist_sq)
- {
- list = list->next;
- }
- }
- rnode->next = list->next;
- list->next = rnode;
- return 0;
- }
- //清除返回結果的集合
- //本質上是個雙鏈表中單鏈表的清理
- static void clear_results(struct kdres *rset)
- {
- struct res_node *tmp, *node = rset->rlist->next;
- while(node)
- {
- tmp = node;
- node = node->next;
- free_resnode(tmp);
- }
- rset->rlist->next = 0;
- }