├── .gitignore ├── CMakeLists.txt ├── COPYRIGHT ├── README.md ├── examples ├── CMakeLists.txt ├── test.c ├── test2.c └── test3.c ├── include └── kdtree.h └── src └── kdtree.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | CMakeFiles 2 | CMakeCache.txt 3 | cmake_install.cmake 4 | install_manifest.txt 5 | Makefile 6 | **/Makefile 7 | *.so 8 | *.o 9 | *.a 10 | *.dll 11 | .svn 12 | .*.swp 13 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required (VERSION 2.8.2) 2 | project (kdtree) 3 | 4 | set (LIBRARY_OUTPUT_PATH lib) 5 | 6 | include_directories (include) 7 | 8 | # lib/libkdtree.so 9 | set (SRCS src/kdtree.cpp) 10 | link_libraries (pthread) 11 | add_library (kdtree SHARED ${SRCS}) 12 | set_target_properties(kdtree PROPERTIES LINKER_LANGUAGE CXX) 13 | 14 | # examples 15 | add_subdirectory(examples) 16 | 17 | # install 18 | INSTALL(TARGETS kdtree 19 | RUNTIME DESTINATION bin 20 | LIBRARY DESTINATION lib 21 | ARCHIVE DESTINATION lib 22 | ) 23 | 24 | INSTALL(FILES include/kdtree.h DESTINATION include) 25 | 26 | 27 | -------------------------------------------------------------------------------- /COPYRIGHT: -------------------------------------------------------------------------------- 1 | Copyright (C) 2007-2009 John Tsiombikas 2 | * Original work 3 | 4 | Copyright (C) 2009 Tamas Nepusz 5 | * Single nearest neighbor search 6 | 7 | Copyright (C) 2010 Scott Deming 8 | * C++ derivative 9 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | KDTree 2 | ====== 3 | 4 | This is very much a work in progress. 5 | 6 | C++ KDTree implementation derived from the C KDTree implementation written 7 | by John Tsiombikas. The original C version is available at 8 | [google code](http://code.google.com/p/kdtree) and the specific version 9 | this was derived from is available at 10 | [github](http://github.com/sdeming/kdtree) under the git tag 'original'. 11 | 12 | The C++ side of this is simply to shore up type casting so it builds with 13 | a C++ compiler. 14 | 15 | License 16 | ------- 17 | 18 | [New BSD license][license] 19 | 20 | Requirements 21 | ------------ 22 | 23 | * cmake 2.8.2+ 24 | * g++ 4.4+ 25 | * gnu make 26 | 27 | Building 28 | -------- 29 | 30 | * cmake -DCMAKE_PREFIX_PATH=/opt/local . 31 | * make 32 | * sudo make install 33 | 34 | Contributing 35 | ------------ 36 | 37 | 1. Fork it. 38 | 2. Create a branch 39 | 3. Commit your changes 40 | 4. Push to the branch 41 | 5. Fire off a pull request 42 | 43 | [license]: http://www.opensource.org/licenses/bsd-license.php 44 | -------------------------------------------------------------------------------- /examples/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required (VERSION 2.8.2) 2 | project (examples) 3 | 4 | include_directories (../include) 5 | 6 | link_libraries(kdtree) 7 | 8 | add_executable(test test.c) 9 | add_executable(test2 test2.c) 10 | add_executable(test3 test3.c) 11 | 12 | -------------------------------------------------------------------------------- /examples/test.c: -------------------------------------------------------------------------------- 1 | /*! gcc -Wall -g -o test test.c libkdtree.a */ 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include "kdtree.h" 9 | 10 | unsigned int get_msec(void) 11 | { 12 | static struct timeval timeval, first_timeval; 13 | 14 | gettimeofday(&timeval, 0); 15 | 16 | if(first_timeval.tv_sec == 0) { 17 | first_timeval = timeval; 18 | return 0; 19 | } 20 | return (timeval.tv_sec - first_timeval.tv_sec) * 1000 + (timeval.tv_usec - first_timeval.tv_usec) / 1000; 21 | } 22 | 23 | 24 | int main(int argc, char **argv) 25 | { 26 | int i, vcount = 10; 27 | void *kd, *set; 28 | unsigned int msec, start; 29 | 30 | if(argc > 1 && isdigit(argv[1][0])) { 31 | vcount = atoi(argv[1]); 32 | } 33 | printf("inserting %d random vectors... ", vcount); 34 | fflush(stdout); 35 | 36 | kd = kd_create(3); 37 | 38 | start = get_msec(); 39 | for(i=0; i 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include "kdtree.h" 10 | 11 | #define DEF_NUM_PTS 10 12 | 13 | /* returns the distance squared between two dims-dimensional double arrays */ 14 | static double dist_sq( double *a1, double *a2, int dims ); 15 | 16 | /* get a random double between -10 and 10 */ 17 | static double rd( void ); 18 | 19 | int main(int argc, char **argv) { 20 | int i, num_pts = DEF_NUM_PTS; 21 | void *ptree; 22 | char *data, *pch; 23 | struct kdres *presults; 24 | double pos[3], dist; 25 | double pt[3] = { 0, 0, 1 }; 26 | double radius = 10; 27 | 28 | if(argc > 1 && isdigit(argv[1][0])) { 29 | num_pts = atoi(argv[1]); 30 | } 31 | 32 | if(!(data = malloc(num_pts))) { 33 | perror("malloc failed"); 34 | return 1; 35 | } 36 | 37 | srand( time(0) ); 38 | 39 | /* create a k-d tree for 3-dimensional points */ 40 | ptree = kd_create( 3 ); 41 | 42 | /* add some random nodes to the tree (assert nodes are successfully inserted) */ 43 | for( i=0; i= 0 ) { 81 | diff = (a1[dims] - a2[dims]); 82 | dist_sq += diff*diff; 83 | } 84 | return dist_sq; 85 | } 86 | 87 | static double rd( void ) { 88 | return (double)rand()/RAND_MAX * 20.0 - 10.0; 89 | } 90 | -------------------------------------------------------------------------------- /examples/test3.c: -------------------------------------------------------------------------------- 1 | /*! gcc -std=c89 -pedantic -Wall -g -o test2 test2.c libkdtree.a -lm */ 2 | /* Extended test program, contributed by David Underhill */ 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include "kdtree.h" 10 | 11 | #define DEF_NUM_PTS 1000000 12 | 13 | /* returns the distance squared between two dims-dimensional double arrays */ 14 | static double dist_sq( double *a1, double *a2, int dims ); 15 | 16 | /* get a random double between -10 and 10 */ 17 | static double rd( void ); 18 | 19 | /* generate an array of random values for dimensions */ 20 | static void generate_dims(int num, double *nums); 21 | 22 | int main(int argc, char **argv) { 23 | int i, num_pts = DEF_NUM_PTS; 24 | void *ptree; 25 | char *data, *pch; 26 | struct kdres *presults; 27 | double pos[50], dist; 28 | double pt[50]; 29 | double nums[50]; 30 | double radius = 40; 31 | 32 | for (i=0; i<50; ++i) { 33 | pt[i] = rd(); 34 | } 35 | 36 | if(!(data = malloc(num_pts))) { 37 | perror("malloc failed"); 38 | return 1; 39 | } 40 | 41 | srand( time(0) ); 42 | 43 | /* create a k-d tree for 50-dimensional points */ 44 | ptree = kd_create(50); 45 | 46 | /* add some random nodes to the tree (assert nodes are successfully inserted) */ 47 | printf("Seeding %d entries of %d dimensions...\n", num_pts, 50); 48 | for( i=0; i= 0 ) { 87 | diff = (a1[dims] - a2[dims]); 88 | dist_sq += diff*diff; 89 | } 90 | return dist_sq; 91 | } 92 | 93 | static double rd( void ) { 94 | return (double)rand()/RAND_MAX * 20.0 - 10.0; 95 | } 96 | 97 | static void generate_dims(int num, double *nums) 98 | { 99 | int i = 0; 100 | for (i=0; i<50; ++i) { 101 | nums[i] = rd(); 102 | } 103 | } 104 | -------------------------------------------------------------------------------- /include/kdtree.h: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of ``kdtree'', a library for working with kd-trees. 3 | Copyright (C) 2007-2009 John Tsiombikas 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 2. Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 3. The name of the author may not be used to endorse or promote products 14 | derived from this software without specific prior written permission. 15 | 16 | THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED 17 | WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF 18 | MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO 19 | EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 20 | EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT 21 | OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 22 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 23 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING 24 | IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY 25 | OF SUCH DAMAGE. 26 | */ 27 | #ifndef _KDTREE_H_ 28 | #define _KDTREE_H_ 29 | 30 | #ifdef __cplusplus 31 | extern "C" { 32 | #endif 33 | 34 | struct kdhyperrect { 35 | int dim; 36 | double *min, *max; /* minimum/maximum coords */ 37 | }; 38 | 39 | struct kdnode { 40 | double *pos; 41 | int dir; 42 | void *data; 43 | 44 | struct kdnode *left, *right; /* negative/positive side */ 45 | }; 46 | 47 | struct res_node { 48 | struct kdnode *item; 49 | double dist_sq; 50 | struct res_node *next; 51 | }; 52 | 53 | struct kdtree { 54 | int dim; 55 | struct kdnode *root; 56 | struct kdhyperrect *rect; 57 | void (*destr)(void*); 58 | }; 59 | 60 | struct kdres { 61 | struct kdtree *tree; 62 | struct res_node *rlist, *riter; 63 | int size; 64 | }; 65 | 66 | 67 | /* create a kd-tree for "k"-dimensional data */ 68 | struct kdtree *kd_create(int k); 69 | 70 | /* free the struct kdtree */ 71 | void kd_free(struct kdtree *tree); 72 | 73 | /* remove all the elements from the tree */ 74 | void kd_clear(struct kdtree *tree); 75 | 76 | /* if called with non-null 2nd argument, the function provided 77 | * will be called on data pointers (see kd_insert) when nodes 78 | * are to be removed from the tree. 79 | */ 80 | void kd_data_destructor(struct kdtree *tree, void (*destr)(void*)); 81 | 82 | /* insert a node, specifying its position, and optional data */ 83 | int kd_insert(struct kdtree *tree, const double *pos, void *data); 84 | 85 | /* Find the nearest node from a given point. 86 | * 87 | * This function returns a pointer to a result set with at most one element. 88 | */ 89 | struct kdres *kd_nearest(struct kdtree *tree, const double *pos); 90 | 91 | /* Find the N nearest nodes from a given point. 92 | * 93 | * This function returns a pointer to a result set, with at most N elements, 94 | * which can be manipulated with the kd_res_* functions. 95 | * The returned pointer can be null as an indication of an error. Otherwise 96 | * a valid result set is always returned which may contain 0 or more elements. 97 | * The result set must be deallocated with kd_res_free after use. 98 | */ 99 | /* 100 | struct kdres *kd_nearest_n(struct kdtree *tree, const double *pos, int num); 101 | struct kdres *kd_nearest_nf(struct kdtree *tree, const float *pos, int num); 102 | struct kdres *kd_nearest_n3(struct kdtree *tree, double x, double y, double z); 103 | struct kdres *kd_nearest_n3f(struct kdtree *tree, float x, float y, float z); 104 | */ 105 | 106 | /* Find any nearest nodes from a given point within a range. 107 | * 108 | * This function returns a pointer to a result set, which can be manipulated 109 | * by the kd_res_* functions. 110 | * The returned pointer can be null as an indication of an error. Otherwise 111 | * a valid result set is always returned which may contain 0 or more elements. 112 | * The result set must be deallocated with kd_res_free after use. 113 | */ 114 | struct kdres *kd_nearest_range(struct kdtree *tree, const double *pos, double range); 115 | 116 | /* frees a result set returned by kd_nearest_range() */ 117 | void kd_res_free(struct kdres *set); 118 | 119 | /* returns the size of the result set (in elements) */ 120 | int kd_res_size(struct kdres *set); 121 | 122 | /* rewinds the result set iterator */ 123 | void kd_res_rewind(struct kdres *set); 124 | 125 | /* returns non-zero if the set iterator reached the end after the last element */ 126 | int kd_res_end(struct kdres *set); 127 | 128 | /* advances the result set iterator, returns non-zero on success, zero if 129 | * there are no more elements in the result set. 130 | */ 131 | int kd_res_next(struct kdres *set); 132 | 133 | /* returns the data pointer (can be null) of the current result set item 134 | * and optionally sets its position to the pointers(s) if not null. 135 | */ 136 | void *kd_res_item(struct kdres *set, double *pos); 137 | 138 | /* equivalent to kd_res_item(set, 0) */ 139 | void *kd_res_item_data(struct kdres *set); 140 | 141 | 142 | #ifdef __cplusplus 143 | } 144 | #endif 145 | 146 | #endif /* _KDTREE_H_ */ 147 | -------------------------------------------------------------------------------- /src/kdtree.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of ``kdtree'', a library for working with kd-trees. 3 | Copyright (C) 2007-2009 John Tsiombikas 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 2. Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 3. The name of the author may not be used to endorse or promote products 14 | derived from this software without specific prior written permission. 15 | 16 | THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED 17 | WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF 18 | MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO 19 | EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 20 | EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT 21 | OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 22 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 23 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING 24 | IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY 25 | OF SUCH DAMAGE. 26 | */ 27 | /* single nearest neighbor search written by Tamas Nepusz */ 28 | 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include "kdtree.h" 35 | 36 | #define SQ(x) ((x) * (x)) 37 | 38 | static void clear_rec(struct kdnode *node, void (*destr)(void*)); 39 | static int insert_rec(struct kdnode **node, const double *pos, void *data, int dir, int dim); 40 | static int rlist_insert(struct res_node *list, struct kdnode *item, double dist_sq); 41 | static void clear_results(struct kdres *set); 42 | 43 | static struct kdhyperrect* hyperrect_create(int dim, const double *min, const double *max); 44 | static void hyperrect_free(struct kdhyperrect *rect); 45 | static struct kdhyperrect* hyperrect_duplicate(const struct kdhyperrect *rect); 46 | static void hyperrect_extend(struct kdhyperrect *rect, const double *pos); 47 | static double hyperrect_dist_sq(struct kdhyperrect *rect, const double *pos); 48 | 49 | struct kdtree *kd_create(int k) 50 | { 51 | struct kdtree *tree; 52 | 53 | if(!(tree = new kdtree)) { 54 | return 0; 55 | } 56 | 57 | tree->dim = k; 58 | tree->root = 0; 59 | tree->destr = 0; 60 | tree->rect = 0; 61 | 62 | return tree; 63 | } 64 | 65 | void kd_free(struct kdtree *tree) 66 | { 67 | if(tree) { 68 | kd_clear(tree); 69 | delete tree; 70 | } 71 | } 72 | 73 | static void clear_rec(struct kdnode *node, void (*destr)(void*)) 74 | { 75 | if(!node) return; 76 | 77 | clear_rec(node->left, destr); 78 | clear_rec(node->right, destr); 79 | 80 | if(destr) { 81 | destr(node->data); 82 | } 83 | delete[] node->pos; 84 | delete node; 85 | } 86 | 87 | void kd_clear(struct kdtree *tree) 88 | { 89 | clear_rec(tree->root, tree->destr); 90 | tree->root = 0; 91 | 92 | if (tree->rect) { 93 | hyperrect_free(tree->rect); 94 | tree->rect = 0; 95 | } 96 | } 97 | 98 | void kd_data_destructor(struct kdtree *tree, void (*destr)(void*)) 99 | { 100 | tree->destr = destr; 101 | } 102 | 103 | 104 | static int insert_rec(struct kdnode **nptr, const double *pos, void *data, int dir, int dim) 105 | { 106 | int new_dir; 107 | struct kdnode *node; 108 | 109 | if(!*nptr) { 110 | if(!(node = new kdnode)) { 111 | return -1; 112 | } 113 | if(!(node->pos = new double[dim])) { 114 | delete[] node; 115 | return -1; 116 | } 117 | memcpy(node->pos, pos, dim * sizeof *node->pos); 118 | node->data = data; 119 | node->dir = dir; 120 | node->left = node->right = 0; 121 | *nptr = node; 122 | return 0; 123 | } 124 | 125 | node = *nptr; 126 | new_dir = (node->dir + 1) % dim; 127 | if(pos[node->dir] < node->pos[node->dir]) { 128 | return insert_rec(&(*nptr)->left, pos, data, new_dir, dim); 129 | } 130 | return insert_rec(&(*nptr)->right, pos, data, new_dir, dim); 131 | } 132 | 133 | int kd_insert(struct kdtree *tree, const double *pos, void *data) 134 | { 135 | if (insert_rec(&tree->root, pos, data, 0, tree->dim)) { 136 | return -1; 137 | } 138 | 139 | if (tree->rect == 0) { 140 | tree->rect = hyperrect_create(tree->dim, pos, pos); 141 | } else { 142 | hyperrect_extend(tree->rect, pos); 143 | } 144 | 145 | return 0; 146 | } 147 | 148 | static int find_nearest(struct kdnode *node, const double *pos, double range, struct res_node *list, int ordered, int dim) 149 | { 150 | double dist_sq, dx; 151 | int i, ret, added_res = 0; 152 | 153 | if(!node) return 0; 154 | 155 | dist_sq = 0; 156 | for(i=0; ipos[i] - pos[i]); 158 | } 159 | if(dist_sq <= SQ(range)) { 160 | if(rlist_insert(list, node, ordered ? dist_sq : -1.0) == -1) { 161 | return -1; 162 | } 163 | added_res = 1; 164 | } 165 | 166 | dx = pos[node->dir] - node->pos[node->dir]; 167 | 168 | ret = find_nearest(dx <= 0.0 ? node->left : node->right, pos, range, list, ordered, dim); 169 | if(ret >= 0 && fabs(dx) < range) { 170 | added_res += ret; 171 | ret = find_nearest(dx <= 0.0 ? node->right : node->left, pos, range, list, ordered, dim); 172 | } 173 | if(ret == -1) { 174 | return -1; 175 | } 176 | added_res += ret; 177 | 178 | return added_res; 179 | } 180 | 181 | #if 0 182 | static int find_nearest_n(struct kdnode *node, const double *pos, double range, int num, struct rheap *heap, int dim) 183 | { 184 | double dist_sq, dx; 185 | int i, ret, added_res = 0; 186 | 187 | if(!node) return 0; 188 | 189 | /* if the photon is close enough, add it to the result heap */ 190 | dist_sq = 0; 191 | for(i=0; ipos[i] - pos[i]); 193 | } 194 | if(dist_sq <= range_sq) { 195 | if(heap->size >= num) { 196 | /* get furthest element */ 197 | struct res_node *maxelem = rheap_get_max(heap); 198 | 199 | /* and check if the new one is closer than that */ 200 | if(maxelem->dist_sq > dist_sq) { 201 | rheap_remove_max(heap); 202 | 203 | if(rheap_insert(heap, node, dist_sq) == -1) { 204 | return -1; 205 | } 206 | added_res = 1; 207 | 208 | range_sq = dist_sq; 209 | } 210 | } else { 211 | if(rheap_insert(heap, node, dist_sq) == -1) { 212 | return =1; 213 | } 214 | added_res = 1; 215 | } 216 | } 217 | 218 | 219 | /* find signed distance from the splitting plane */ 220 | dx = pos[node->dir] - node->pos[node->dir]; 221 | 222 | ret = find_nearest_n(dx <= 0.0 ? node->left : node->right, pos, range, num, heap, dim); 223 | if(ret >= 0 && fabs(dx) < range) { 224 | added_res += ret; 225 | ret = find_nearest_n(dx <= 0.0 ? node->right : node->left, pos, range, num, heap, dim); 226 | } 227 | 228 | } 229 | #endif 230 | 231 | static void kd_nearest_i(struct kdnode *node, const double *pos, struct kdnode **result, double *result_dist_sq, struct kdhyperrect* rect) 232 | { 233 | int dir = node->dir; 234 | int i; 235 | double dummy, dist_sq; 236 | struct kdnode *nearer_subtree, *farther_subtree; 237 | double *nearer_hyperrect_coord, *farther_hyperrect_coord; 238 | 239 | /* Decide whether to go left or right in the tree */ 240 | dummy = pos[dir] - node->pos[dir]; 241 | if (dummy <= 0) { 242 | nearer_subtree = node->left; 243 | farther_subtree = node->right; 244 | nearer_hyperrect_coord = rect->max + dir; 245 | farther_hyperrect_coord = rect->min + dir; 246 | } else { 247 | nearer_subtree = node->right; 248 | farther_subtree = node->left; 249 | nearer_hyperrect_coord = rect->min + dir; 250 | farther_hyperrect_coord = rect->max + dir; 251 | } 252 | 253 | if (nearer_subtree) { 254 | /* Slice the hyperrect to get the hyperrect of the nearer subtree */ 255 | dummy = *nearer_hyperrect_coord; 256 | *nearer_hyperrect_coord = node->pos[dir]; 257 | /* Recurse down into nearer subtree */ 258 | kd_nearest_i(nearer_subtree, pos, result, result_dist_sq, rect); 259 | /* Undo the slice */ 260 | *nearer_hyperrect_coord = dummy; 261 | } 262 | 263 | /* Check the distance of the point at the current node, compare it 264 | * with our best so far */ 265 | dist_sq = 0; 266 | for(i=0; i < rect->dim; i++) { 267 | dist_sq += SQ(node->pos[i] - pos[i]); 268 | } 269 | if (dist_sq < *result_dist_sq) { 270 | *result = node; 271 | *result_dist_sq = dist_sq; 272 | } 273 | 274 | if (farther_subtree) { 275 | /* Get the hyperrect of the farther subtree */ 276 | dummy = *farther_hyperrect_coord; 277 | *farther_hyperrect_coord = node->pos[dir]; 278 | /* Check if we have to recurse down by calculating the closest 279 | * point of the hyperrect and see if it's closer than our 280 | * minimum distance in result_dist_sq. */ 281 | if (hyperrect_dist_sq(rect, pos) < *result_dist_sq) { 282 | /* Recurse down into farther subtree */ 283 | kd_nearest_i(farther_subtree, pos, result, result_dist_sq, rect); 284 | } 285 | /* Undo the slice on the hyperrect */ 286 | *farther_hyperrect_coord = dummy; 287 | } 288 | } 289 | 290 | struct kdres *kd_nearest(struct kdtree *kd, const double *pos) 291 | { 292 | struct kdhyperrect *rect; 293 | struct kdnode *result; 294 | struct kdres *rset; 295 | double dist_sq; 296 | int i; 297 | 298 | if (!kd) return 0; 299 | if (!kd->rect) return 0; 300 | 301 | /* Allocate result set */ 302 | if(!(rset = new kdres)) { 303 | return 0; 304 | } 305 | if(!(rset->rlist = new res_node)) { 306 | delete rset; 307 | return 0; 308 | } 309 | rset->rlist->next = 0; 310 | rset->tree = kd; 311 | 312 | /* Duplicate the bounding hyperrectangle, we will work on the copy */ 313 | if (!(rect = hyperrect_duplicate(kd->rect))) { 314 | kd_res_free(rset); 315 | return 0; 316 | } 317 | 318 | /* Our first guesstimate is the root node */ 319 | result = kd->root; 320 | dist_sq = 0; 321 | for (i = 0; i < kd->dim; i++) 322 | dist_sq += SQ(result->pos[i] - pos[i]); 323 | 324 | /* Search for the nearest neighbour recursively */ 325 | kd_nearest_i(kd->root, pos, &result, &dist_sq, rect); 326 | 327 | /* Free the copy of the hyperrect */ 328 | hyperrect_free(rect); 329 | 330 | /* Store the result */ 331 | if (result) { 332 | if (rlist_insert(rset->rlist, result, -1.0) == -1) { 333 | kd_res_free(rset); 334 | return 0; 335 | } 336 | rset->size = 1; 337 | kd_res_rewind(rset); 338 | return rset; 339 | } else { 340 | kd_res_free(rset); 341 | return 0; 342 | } 343 | } 344 | 345 | struct kdres *kd_nearest_range(struct kdtree *kd, const double *pos, double range) 346 | { 347 | int ret; 348 | struct kdres *rset; 349 | 350 | if(!(rset = new kdres)) { 351 | return 0; 352 | } 353 | if(!(rset->rlist = new res_node)) { 354 | delete rset; 355 | return 0; 356 | } 357 | rset->rlist->next = 0; 358 | rset->tree = kd; 359 | 360 | if((ret = find_nearest(kd->root, pos, range, rset->rlist, 0, kd->dim)) == -1) { 361 | kd_res_free(rset); 362 | return 0; 363 | } 364 | rset->size = ret; 365 | kd_res_rewind(rset); 366 | return rset; 367 | } 368 | 369 | void kd_res_free(struct kdres *rset) 370 | { 371 | clear_results(rset); 372 | delete rset->rlist; 373 | delete rset; 374 | } 375 | 376 | int kd_res_size(struct kdres *set) 377 | { 378 | return (set->size); 379 | } 380 | 381 | void kd_res_rewind(struct kdres *rset) 382 | { 383 | rset->riter = rset->rlist->next; 384 | } 385 | 386 | int kd_res_end(struct kdres *rset) 387 | { 388 | return rset->riter == 0; 389 | } 390 | 391 | int kd_res_next(struct kdres *rset) 392 | { 393 | rset->riter = rset->riter->next; 394 | return rset->riter != 0; 395 | } 396 | 397 | void *kd_res_item(struct kdres *rset, double *pos) 398 | { 399 | if(rset->riter) { 400 | if(pos) { 401 | memcpy(pos, rset->riter->item->pos, rset->tree->dim * sizeof *pos); 402 | } 403 | return rset->riter->item->data; 404 | } 405 | return 0; 406 | } 407 | 408 | void *kd_res_item_data(struct kdres *set) 409 | { 410 | return kd_res_item(set, 0); 411 | } 412 | 413 | /* ---- hyperrectangle helpers ---- */ 414 | static struct kdhyperrect* hyperrect_create(int dim, const double *min, const double *max) 415 | { 416 | size_t size = dim * sizeof(double); 417 | struct kdhyperrect* rect = 0; 418 | 419 | if (!(rect = new kdhyperrect)) { 420 | return 0; 421 | } 422 | 423 | rect->dim = dim; 424 | if (!(rect->min = new double[size])) { 425 | delete rect; 426 | return 0; 427 | } 428 | if (!(rect->max = new double[size])) { 429 | delete[] rect->min; 430 | delete rect; 431 | return 0; 432 | } 433 | memcpy(rect->min, min, size); 434 | memcpy(rect->max, max, size); 435 | 436 | return rect; 437 | } 438 | 439 | static void hyperrect_free(struct kdhyperrect *rect) 440 | { 441 | delete[] rect->min; 442 | delete[] rect->max; 443 | delete rect; 444 | } 445 | 446 | static struct kdhyperrect* hyperrect_duplicate(const struct kdhyperrect *rect) 447 | { 448 | return hyperrect_create(rect->dim, rect->min, rect->max); 449 | } 450 | 451 | static void hyperrect_extend(struct kdhyperrect *rect, const double *pos) 452 | { 453 | int i; 454 | 455 | for (i=0; i < rect->dim; i++) { 456 | if (pos[i] < rect->min[i]) { 457 | rect->min[i] = pos[i]; 458 | } 459 | if (pos[i] > rect->max[i]) { 460 | rect->max[i] = pos[i]; 461 | } 462 | } 463 | } 464 | 465 | static double hyperrect_dist_sq(struct kdhyperrect *rect, const double *pos) 466 | { 467 | int i; 468 | double result = 0; 469 | 470 | for (i=0; i < rect->dim; i++) { 471 | if (pos[i] < rect->min[i]) { 472 | result += SQ(rect->min[i] - pos[i]); 473 | } else if (pos[i] > rect->max[i]) { 474 | result += SQ(rect->max[i] - pos[i]); 475 | } 476 | } 477 | 478 | return result; 479 | } 480 | 481 | /* inserts the item. if dist_sq is >= 0, then do an ordered insert */ 482 | /* TODO make the ordering code use heapsort */ 483 | static int rlist_insert(struct res_node *list, struct kdnode *item, double dist_sq) 484 | { 485 | struct res_node *rnode; 486 | 487 | if(!(rnode = new res_node)) { 488 | return -1; 489 | } 490 | rnode->item = item; 491 | rnode->dist_sq = dist_sq; 492 | 493 | if(dist_sq >= 0.0) { 494 | while(list->next && list->next->dist_sq < dist_sq) { 495 | list = list->next; 496 | } 497 | } 498 | rnode->next = list->next; 499 | list->next = rnode; 500 | return 0; 501 | } 502 | 503 | static void clear_results(struct kdres *rset) 504 | { 505 | struct res_node *tmp, *node = rset->rlist->next; 506 | 507 | while(node) { 508 | tmp = node; 509 | node = node->next; 510 | delete tmp; 511 | } 512 | 513 | rset->rlist->next = 0; 514 | } 515 | --------------------------------------------------------------------------------