Back to index

python-biopython  1.60
KDTree.c
Go to the documentation of this file.
00001 #include <math.h>
00002 #include <stdlib.h>
00003 #include "KDTree.h"
00004 
00005 #define INF 1000000
00006 
00007 struct DataPoint;
00008 
00009 struct Node;
00010 
00011 struct Region;
00012 
00013 struct Radius;
00014 
00015 struct KDTree
00016 {
00017     struct DataPoint* _data_point_list;
00018     int _data_point_list_size;
00019     struct Radius* _radius_list;
00020     struct Neighbor* _neighbor_list;
00021     struct Node *_root;
00022     struct Region *_query_region;
00023     long int _count;
00024     long int _neighbor_count;
00025     float _radius;
00026     float _radius_sq;
00027     float _neighbor_radius;
00028     float _neighbor_radius_sq;
00029     float *_center_coord;
00030     float *_coords;
00031     int _bucket_size;
00032     int dim;
00033 };
00034 
00035 /* DataPoint */
00036 
00037 static int DataPoint_current_dim=0;
00038 
00039 struct DataPoint
00040 {
00041     long int _index;
00042     float *_coord;
00043 };
00044 
00045 static int compare(const void* self, const void* other)
00046 {
00047     float a, b;
00048     const struct DataPoint* p = (const struct DataPoint*)self;
00049     const struct DataPoint* q = (const struct DataPoint*)other;
00050 
00051     a=p->_coord[DataPoint_current_dim];
00052     b=q->_coord[DataPoint_current_dim];
00053 
00054     if (a < b) return -1;
00055     if (a > b) return +1;
00056     return 0; 
00057 }
00058 
00059 static void DataPoint_sort(struct DataPoint* list, int n, int i)
00060 {
00061     /* set sort dimension */
00062     DataPoint_current_dim=i;
00063     qsort(list, n, sizeof(struct DataPoint), compare);
00064 }
00065 
00066 /* Node */
00067 
00068 struct Node
00069 {
00070     struct Node *_left;
00071     struct Node *_right;
00072     float _cut_value;
00073     int _cut_dim;
00074     long int _start, _end;
00075 };
00076 
00077 static struct Node*
00078 Node_create(float cut_value, int cut_dim, long int start, long int end)
00079 {
00080     struct Node* node = malloc(sizeof(struct Node));
00081     if (node==NULL) return NULL;
00082     node->_left=NULL;
00083     node->_right=NULL;
00084     node->_cut_value=cut_value;
00085     node->_cut_dim=cut_dim;
00086     /* start and end index in _data_point_list */
00087     node->_start=start;
00088     node->_end=end;
00089     return node;
00090 }
00091 
00092 static void Node_destroy(struct Node* node)
00093 {
00094     if(node==NULL) return;
00095     Node_destroy(node->_left);
00096     Node_destroy(node->_right);
00097     free(node);
00098 }
00099 
00100 static int Node_is_leaf(struct Node* node)
00101 {
00102     if (node->_left==NULL && node->_right==NULL) return 1;
00103     else return 0;
00104 }
00105 
00106 /* Region */
00107 
00108 static int Region_dim=3;
00109 
00110 struct Region
00111 {
00112     float *_left; 
00113     float *_right;
00114 };
00115 
00116 static struct Region* Region_create(const float *left, const float *right)
00117 {
00118     struct Region* region = malloc(sizeof(struct Region));
00119     if(!region) return NULL;
00120 
00121     region->_left= malloc(Region_dim*sizeof(float));
00122     region->_right= malloc(Region_dim*sizeof(float));
00123     if (region->_left==NULL || region->_right==NULL)
00124     {
00125         if (region->_left) free(region->_left);
00126         if (region->_right) free(region->_right);
00127         free(region);
00128         return NULL;
00129     }
00130 
00131     if (left==NULL || right==NULL)
00132     {
00133         /* [-INF, INF] */
00134         int i;
00135         for (i=0; i<Region_dim; i++)
00136         {
00137             region->_left[i]=-INF;
00138             region->_right[i]=INF;
00139         }
00140     }
00141     else
00142     {
00143         int i;
00144         for (i=0; i<Region_dim; i++)
00145         {
00146             region->_left[i]=left[i];
00147             region->_right[i]=right[i];
00148         }
00149     }
00150     return region;
00151 }
00152 
00153 static void Region_destroy(struct Region* region)
00154 {
00155     if(region==NULL) return;
00156     if(region->_left) free(region->_left);
00157     if(region->_right) free(region->_right);
00158     free(region);
00159 }
00160 
00161 static struct Region*
00162 Region_create_intersect_left(struct Region* region, float split_coord, int current_dim)
00163 {
00164     struct Region* p;
00165     const float value = region->_right[current_dim];
00166     region->_right[current_dim]=split_coord;
00167     p = Region_create(region->_left, region->_right);
00168     region->_right[current_dim] = value;
00169     return p;
00170 }
00171 
00172 static struct Region*
00173 Region_create_intersect_right(struct Region* region, float split_coord, int current_dim)
00174 {
00175     struct Region* p;
00176     const float value = region->_left[current_dim];
00177     region->_left[current_dim]=split_coord;
00178     p = Region_create(region->_left, region->_right);
00179     region->_left[current_dim]=value;
00180     return p;
00181 }
00182 
00183 static int
00184 Region_test_intersect_left(struct Region* region, float split_coord, int current_dim)
00185 {
00186     float l, r;
00187 
00188     r=region->_right[current_dim];
00189     l=region->_left[current_dim];
00190 
00191     if (split_coord<l) return -1;
00192     else if (split_coord<r) return 0; /* split point in interval */
00193     else return +1;
00194 }
00195 
00196 static int
00197 Region_test_intersect_right(struct Region* region, float split_coord, int current_dim)
00198 {
00199     float l, r;
00200 
00201     r=region->_right[current_dim];
00202     l=region->_left[current_dim];
00203 
00204     if (split_coord<=l) return -1;
00205     else if (split_coord<=r) return 0; /* split point in interval */
00206     else return +1;
00207 }
00208 
00209 static int Region_encloses(struct Region* region, float *coord)
00210 {
00211     int i;
00212     for (i=0; i<Region_dim; i++)
00213     {
00214         if (!(coord[i]>=region->_left[i] && coord[i]<=region->_right[i]))
00215         {
00216             return 0;
00217         }
00218     }
00219     return 1;
00220 }
00221 
00222 static int
00223 Region_test_intersection(struct Region* this_region, struct Region *query_region, float radius)
00224 {
00225     int status=2;
00226 
00227     int i;
00228     for (i=0; i<Region_dim; i++)
00229     {
00230         float rs, rq, ls, lq;
00231 
00232         rs=this_region->_right[i];
00233         ls=this_region->_left[i];
00234         rq=query_region->_right[i];
00235         lq=query_region->_left[i];
00236 
00237         if (ls-rq>radius)
00238         {
00239             /* outside */
00240             return 0;
00241         }
00242         else if (lq-rs>radius)
00243         {
00244             /* outside */
00245             return 0;
00246         }
00247         else if (rs<=rq && ls>=lq)
00248         {
00249             /* inside (at least in dim i) */
00250             if (status > 2) status=2;
00251         }
00252         else
00253         {
00254             /* overlap (at least in dim i) */
00255             status=1;
00256         }
00257     }
00258     return status;
00259 }
00260 
00261 /* Radius */
00262 
00263 struct Radius
00264 {
00265     long int index;
00266     float value;
00267 };
00268 
00269 /* KDTree */
00270 
00271 struct KDTree* KDTree_init(int dim, int bucket_size)
00272 {
00273     struct KDTree* tree;
00274 
00275     tree = malloc(sizeof(struct KDTree));
00276     if (!tree) return NULL;
00277 
00278     tree->_center_coord= malloc(dim*sizeof(float));
00279     if (tree->_center_coord==NULL)
00280     {
00281         free(tree);
00282         return NULL;
00283     }
00284 
00285     tree->dim=dim;
00286 
00287     Region_dim=tree->dim;
00288 
00289     tree->_query_region=NULL;
00290     tree->_root=NULL;
00291     tree->_coords=NULL;
00292     tree->_radius_list = NULL;
00293     tree->_count=0;
00294     tree->_neighbor_count=0;
00295     tree->_neighbor_list = NULL;
00296     tree->_bucket_size=bucket_size;
00297     tree->_data_point_list = NULL;
00298     tree->_data_point_list_size = 0;
00299 
00300     return tree;
00301 }
00302 
00303 void KDTree_destroy(struct KDTree* tree)
00304 {
00305     /* clean up KD tree */
00306     if (!tree) return;
00307     Node_destroy(tree->_root);
00308     Region_destroy(tree->_query_region);
00309     if (tree->_center_coord) free(tree->_center_coord);
00310     if (tree->_coords) free(tree->_coords);
00311     if (tree->_data_point_list) free(tree->_data_point_list);
00312     if (tree->_neighbor_list) free(tree->_neighbor_list);
00313     free(tree);
00314 }
00315 
00316 static struct Node *KDTree_build_tree(struct KDTree* tree, long int offset_begin, long int offset_end, int depth)
00317 {
00318     int localdim;
00319 
00320     if (depth==0)
00321     {
00322         /* start with [begin, end+1] */
00323         offset_begin=0;
00324         offset_end=tree->_data_point_list_size;
00325         localdim=0;
00326     }
00327     else
00328     {
00329         localdim=depth%tree->dim;
00330     }
00331 
00332     if ((offset_end-offset_begin)<=tree->_bucket_size) 
00333     {
00334         /* leaf node */
00335         return Node_create(-1, localdim, offset_begin, offset_end);
00336     }
00337     else
00338     {
00339         long int offset_split;
00340         long int left_offset_begin, left_offset_end;
00341         long int right_offset_begin, right_offset_end;
00342         long int d;
00343         float cut_value;
00344         struct DataPoint data_point;
00345         struct Node *left_node, *right_node, *new_node;
00346 
00347         DataPoint_sort(tree->_data_point_list+offset_begin, offset_end-offset_begin, localdim);
00348 
00349         /* calculate index of split point */
00350         d=offset_end-offset_begin;
00351         offset_split=d/2+d%2;
00352 
00353         data_point=tree->_data_point_list[offset_begin+offset_split-1];
00354         cut_value = data_point._coord[localdim];
00355 
00356         /* create new node and bind to left & right nodes */
00357         new_node=Node_create(cut_value, localdim, offset_begin, offset_end);
00358         if (new_node==NULL) return NULL;
00359 
00360         /* left */
00361         left_offset_begin=offset_begin;
00362         left_offset_end=offset_begin+offset_split;
00363         left_node=KDTree_build_tree(tree, left_offset_begin, left_offset_end, depth+1);
00364 
00365         /* right */
00366         right_offset_begin=left_offset_end;
00367         right_offset_end=offset_end;
00368         right_node=KDTree_build_tree(tree, right_offset_begin, right_offset_end, depth+1);
00369 
00370         new_node->_left = left_node;
00371         new_node->_right = right_node;
00372 
00373         if (left_node==NULL || right_node==NULL)
00374         {
00375             Node_destroy(new_node);
00376             return NULL;
00377         }
00378 
00379         return new_node;
00380     }
00381 }
00382 
00383 static int KDTree_add_point(struct KDTree* tree, long int index, float *coord)
00384 {
00385     int n;
00386     struct DataPoint* p;
00387 
00388     n = tree->_data_point_list_size;
00389     p = realloc(tree->_data_point_list, (n+1)*sizeof(struct DataPoint));
00390     if (p==NULL) return 0;
00391 
00392     p[n]._index = index;
00393     p[n]._coord = coord;
00394 
00395     tree->_data_point_list_size = n+1;
00396     tree->_data_point_list = p;
00397 
00398     return 1;
00399 }
00400 
00401 static float KDTree_dist(float *coord1, float *coord2, int dim)
00402 {
00403     /* returns the SQUARE of the distance between two points */
00404     int i;
00405     float sum=0, dif=0;
00406 
00407     for (i=0; i<dim; i++)
00408     {
00409         dif=coord1[i]-coord2[i];
00410         sum+=dif*dif;
00411     }
00412     return sum;
00413 }
00414 
00415 static int KDTree_report_point(struct KDTree* tree, long int index, float *coord)
00416 {
00417     float r;
00418 
00419     r=KDTree_dist(tree->_center_coord, coord, tree->dim);
00420 
00421     if (r<=tree->_radius_sq)
00422     {
00423         int n = tree->_count;
00424         struct Radius* p;
00425 
00426     p  = realloc(tree->_radius_list, (n+1)*sizeof(struct Radius));
00427         if (p==NULL)
00428         {
00429             return 0;
00430         }
00431         /* note use of sqrt - only calculated if necessary */
00432         p[n].index = index;
00433         p[n].value = sqrt(r);
00434         tree->_radius_list = p;
00435         tree->_count++;
00436     }
00437     return 1;
00438 }
00439 
00440 static int KDTree_report_subtree(struct KDTree* tree, struct Node *node)
00441 {
00442     int ok;
00443     if (Node_is_leaf(node))
00444     {
00445         /* report point(s) */
00446         long int i;
00447 
00448         for (i=node->_start; i<node->_end; i++)
00449         {
00450             struct DataPoint data_point;
00451             data_point=tree->_data_point_list[i];
00452             ok = KDTree_report_point(tree, data_point._index, data_point._coord);
00453             if (!ok) return 0;
00454         }
00455     }
00456     else
00457     {
00458         /* find points in subtrees via recursion */
00459         ok = KDTree_report_subtree(tree, node->_left);
00460         if (!ok) return 0;
00461         ok = KDTree_report_subtree(tree, node->_right);
00462         if (!ok) return 0;
00463     }
00464     return 1;
00465 }
00466 
00467 long int KDTree_get_count(struct KDTree* tree)
00468 {       
00469     return tree->_count;
00470 }
00471 
00472 long int KDTree_neighbor_get_count(struct KDTree* tree)
00473 {       
00474     return tree->_neighbor_count;
00475 }
00476 
00477 static int KDTree_search(struct KDTree* tree, struct Region *region, struct Node *node, int depth);
00478 
00479 static int KDTree_test_region(struct KDTree* tree, struct Node *node, struct Region *region, int depth)
00480 {
00481     int ok;
00482     int intersect_flag;
00483 
00484     /* is node region inside, outside or overlapping 
00485      * with query region? */
00486     intersect_flag=Region_test_intersection(region, tree->_query_region, 0);
00487 
00488     if (intersect_flag==2)                
00489     {
00490         /* inside - extract points */
00491         ok = KDTree_report_subtree(tree, node);
00492         /* end of recursion -- get rid of region */
00493         Region_destroy(region);
00494         if (!ok) return 0;
00495     }
00496     else if (intersect_flag==1)
00497     {
00498         /* overlap - recursion */
00499         ok = KDTree_search(tree, region, node, depth+1);    
00500         /* search does cleanup of region */
00501         if (!ok) return 0;
00502     }
00503     else
00504     {
00505         /* outside - stop */
00506 
00507         /* end of recursion -- get rid of region */
00508         Region_destroy(region);    
00509     }
00510     return 1;
00511 }
00512 
00513 static int KDTree_search(struct KDTree* tree, struct Region *region, struct Node *node, int depth)
00514 {
00515     int current_dim;
00516     int ok = 1;
00517 
00518     if(depth==0)
00519     {
00520         /* start with [-INF, INF] region */
00521         
00522         region = Region_create(NULL, NULL);
00523         if (region==NULL) return 0;
00524 
00525         /* start with root node */
00526         node=tree->_root;
00527     }
00528 
00529     current_dim=depth%tree->dim;
00530 
00531     if(Node_is_leaf(node))
00532     {
00533         long int i;
00534 
00535         for (i=node->_start; i<node->_end; i++)
00536         {
00537             struct DataPoint data_point;
00538 
00539             data_point=tree->_data_point_list[i];
00540 
00541             if (Region_encloses(tree->_query_region, data_point._coord))
00542             {
00543                 /* point is enclosed in query region - report & stop */
00544                 ok = KDTree_report_point(tree, data_point._index, data_point._coord);
00545             }
00546         }
00547     }
00548     else
00549     {
00550         struct Node *left_node, *right_node;
00551         struct Region *left_region, *right_region;
00552         int intersect_left, intersect_right;
00553 
00554         left_node=node->_left;
00555 
00556         /* LEFT HALF PLANE */
00557 
00558         /* new region */
00559         intersect_left=Region_test_intersect_left(region, node->_cut_value, current_dim);
00560 
00561         if(intersect_left==1)
00562         {
00563             left_region = Region_create(region->_left, region->_right);
00564             if (left_region)
00565                 ok = KDTree_test_region(tree, left_node, left_region, depth);
00566             else
00567                 ok = 0;
00568         }
00569         else if (intersect_left==0)
00570         {
00571             left_region = Region_create_intersect_left(region, node->_cut_value, current_dim);
00572             if (left_region)
00573                 ok = KDTree_test_region(tree, left_node, left_region, depth);
00574             else
00575                 ok = 0;
00576         }
00577         /* intersect_left is -1 if no overlap */
00578 
00579         /* RIGHT HALF PLANE */
00580 
00581         right_node=node->_right;
00582 
00583         /* new region */
00584         intersect_right=Region_test_intersect_right(region, node->_cut_value, current_dim);
00585         if (intersect_right==-1)
00586         {
00587             right_region = Region_create(region->_left, region->_right);
00588             /* test for overlap/inside/outside & do recursion/report/stop */
00589             if (right_region)
00590                 ok = KDTree_test_region(tree, right_node, right_region, depth);
00591             else
00592                 ok = 0;
00593         }
00594         else if (intersect_right==0)
00595         {
00596             right_region = Region_create_intersect_right(region, node->_cut_value, current_dim);
00597             /* test for overlap/inside/outside & do recursion/report/stop */
00598             if (right_region)
00599                 ok = KDTree_test_region(tree, right_node, right_region, depth);
00600             else
00601                 ok = 0;
00602         }
00603         /* intersect_right is +1 if no overlap */
00604     }
00605 
00606     Region_destroy(region);
00607     return ok;
00608 }
00609 
00610 int KDTree_set_data(struct KDTree* tree, float *coords, long int nr_points)
00611 {
00612     long int i;
00613     int ok;
00614 
00615     Region_dim=tree->dim;
00616 
00617     /* clean up stuff from previous use */
00618     Node_destroy(tree->_root);
00619     if (tree->_coords) free(tree->_coords);
00620     if (tree->_radius_list)
00621     {
00622         free(tree->_radius_list);
00623         tree->_radius_list = NULL;
00624     }
00625     tree->_count=0;
00626     /* keep pointer to coords to delete it */
00627     tree->_coords=coords;
00628         
00629     for (i=0; i<nr_points; i++)
00630     {
00631         ok = KDTree_add_point(tree, i, coords+i*tree->dim);
00632         if (!ok) 
00633         {
00634             free(tree->_data_point_list);
00635             tree->_data_point_list = NULL;
00636             tree->_data_point_list_size = 0;
00637             return 0;
00638         }
00639     }
00640 
00641     /* build KD tree */
00642     tree->_root=KDTree_build_tree(tree, 0, 0, 0);
00643     if(!tree->_root) return 0;
00644     return 1;
00645 }
00646 
00647 int KDTree_search_center_radius(struct KDTree* tree, float *coord, float radius)
00648 {
00649     int i;
00650     int dim = tree->dim;
00651     float* left = malloc(dim*sizeof(float));
00652     float* right = malloc(dim*sizeof(float));
00653     if (left==NULL || right==NULL)
00654     {
00655         if (left) free(left);
00656         if (right) free(right);
00657         return 0;
00658     }
00659 
00660     Region_dim=tree->dim;
00661 
00662     if (tree->_radius_list)
00663     {
00664         free(tree->_radius_list);
00665         tree->_radius_list = NULL;
00666     }
00667     tree->_count=0;
00668 
00669     tree->_radius=radius;
00670     /* use of r^2 to avoid sqrt use */
00671     tree->_radius_sq=radius*radius;
00672 
00673     for (i=0; i<tree->dim; i++)
00674     {
00675         left[i]=coord[i]-radius;
00676         right[i]=coord[i]+radius;
00677         /* set center of query */
00678         tree->_center_coord[i]=coord[i];
00679     }
00680 
00681     /* clean up! */
00682     if (coord) free(coord);
00683 
00684     Region_destroy(tree->_query_region);
00685     tree->_query_region= Region_create(left, right);
00686 
00687     free(left);
00688     free(right);
00689 
00690     if (!tree->_query_region) return 0;
00691 
00692     return KDTree_search(tree, NULL, NULL, 0);
00693 }
00694 
00695 void KDTree_copy_indices(struct KDTree* tree, long *indices)
00696 {
00697     long int i;
00698 
00699     if (tree->_count==0) return;
00700 
00701     for(i=0; i<tree->_count; i++)
00702     {
00703         indices[i]=tree->_radius_list[i].index;
00704     }
00705 }
00706 
00707 void KDTree_copy_radii(struct KDTree* tree, float *radii)
00708 {
00709     long int i;
00710 
00711     if (tree->_count==0) return;
00712 
00713     for(i=0; i<tree->_count; i++)
00714     {
00715         radii[i]=tree->_radius_list[i].value;
00716     }
00717 }
00718 
00719 static int KDTree_test_neighbors(struct KDTree* tree, struct DataPoint* p1, struct DataPoint* p2)
00720 {
00721     float r;
00722 
00723     r=KDTree_dist(p1->_coord, p2->_coord, tree->dim);
00724 
00725     if(r<=tree->_neighbor_radius_sq)
00726     {
00727         /* we found a neighbor pair! */
00728         struct Neighbor* p;
00729         int n;
00730         n = tree->_neighbor_count;
00731         p = realloc(tree->_neighbor_list, (n+1)*sizeof(struct Neighbor));
00732         if (p==NULL) return 0;
00733 
00734         p[n].index1 = p1->_index;
00735         p[n].index2 = p2->_index;
00736         /* note sqrt */
00737         p[n].radius = sqrt(r);
00738         tree->_neighbor_list = p;
00739         tree->_neighbor_count++;
00740     }
00741 
00742     return 1;
00743 }
00744 
00745 static int KDTree_search_neighbors_between_buckets(struct KDTree* tree, struct Node *node1, struct Node *node2)
00746 {
00747     long int i;
00748     int ok;
00749 
00750     for(i=node1->_start; i<node1->_end; i++)
00751     {
00752         struct DataPoint p1;
00753         long int j;
00754 
00755         p1=tree->_data_point_list[i];
00756 
00757         for (j=node2->_start; j<node2->_end; j++)
00758         {
00759             struct DataPoint p2;
00760 
00761             p2=tree->_data_point_list[j];
00762 
00763             ok = KDTree_test_neighbors(tree, &p1, &p2);
00764             if (!ok) return 0;
00765         }
00766     }
00767     return 1;
00768 }
00769 
00770 static int KDTree_neighbor_search_pairs(struct KDTree* tree, struct Node *down, struct Region *down_region, struct Node *up, struct Region *up_region, int depth)
00771 {
00772     int down_is_leaf, up_is_leaf;
00773     int localdim;
00774     int ok = 1;
00775 
00776     /* if regions do not overlap - STOP */
00777     if (!down || !up || !down_region || !up_region) 
00778     {
00779         /* STOP */
00780         return ok;
00781     }
00782     
00783     if (Region_test_intersection(down_region, up_region, tree->_neighbor_radius)==0)
00784     {
00785         /* regions cannot contain neighbors */
00786         return ok;
00787     }
00788 
00789     /* dim */
00790     localdim=depth%tree->dim;
00791 
00792     /* are they leaves? */
00793     up_is_leaf=Node_is_leaf(up);
00794     down_is_leaf=Node_is_leaf(down);
00795 
00796     if (up_is_leaf && down_is_leaf)
00797     {
00798         /* two leaf nodes */
00799         ok = KDTree_search_neighbors_between_buckets(tree, down, up);
00800     }
00801     else
00802     {
00803         /* one or no leaf nodes */
00804 
00805         struct Node *up_right, *up_left, *down_left, *down_right;
00806         struct Region *up_left_region = NULL;
00807         struct Region *up_right_region = NULL;
00808         struct Region *down_left_region = NULL;
00809         struct Region *down_right_region = NULL;  
00810         
00811         if (down_is_leaf)
00812         {
00813             down_left=down;
00814             /* make a copy of down_region */
00815             down_left_region= Region_create(down_region->_left, down_region->_right);
00816             if (down_left_region==NULL) ok = 0;
00817             down_right=NULL;
00818             down_right_region=NULL;
00819         }
00820         else
00821         {
00822             float cut_value;
00823             int intersect;
00824 
00825             cut_value=down->_cut_value;
00826 
00827             down_left=down->_left;
00828             down_right=down->_right;
00829             intersect=Region_test_intersect_left(down_region, cut_value, localdim);
00830 
00831             if(intersect==1)
00832             {
00833                 down_left_region = Region_create(down_region->_left, down_region->_right);
00834                 if (down_left_region==NULL) ok = 0;
00835             }
00836             else if(intersect==0)
00837             {
00838                 down_left_region = Region_create_intersect_left(down_region, cut_value, localdim);
00839                 if (down_left_region==NULL) ok = 0;
00840             }
00841             else if(intersect==-1)
00842             /* intersect is -1 if no overlap */
00843             {
00844                 down_left_region = NULL;
00845             }
00846 
00847             intersect=Region_test_intersect_right(down_region, cut_value, localdim); 
00848             if(intersect==-1)
00849             {
00850                 down_right_region = Region_create(down_region->_left, down_region->_right);
00851                 if (down_right_region==NULL) ok = 0;
00852             }
00853             else if(intersect==0)
00854             {
00855                 down_right_region = Region_create_intersect_right(down_region, cut_value, localdim);
00856                 if (down_right_region==NULL) ok = 0;
00857             }
00858             else if(intersect==+1)
00859             {
00860                 down_right_region = NULL;
00861             }
00862         }
00863 
00864         if (up_is_leaf)
00865         {
00866             up_left=up;
00867             /* make a copy of up_region */
00868             up_left_region= Region_create(up_region->_left, up_region->_right);
00869             if (up_left_region==NULL) ok = 0;
00870             up_right=NULL;
00871             up_right_region=NULL;
00872         }
00873         else
00874         {
00875             float cut_value;
00876             int intersect;
00877 
00878             cut_value=up->_cut_value;
00879 
00880             up_left=up->_left;
00881             up_right=up->_right;
00882             intersect=Region_test_intersect_left(up_region, cut_value, localdim);
00883 
00884             if(intersect==1)
00885             {
00886                 up_left_region = Region_create(up_region->_left, up_region->_right);
00887                 if (up_left_region==NULL) ok = 0;
00888             }
00889             else if(intersect==0)
00890             {
00891                 up_left_region = Region_create_intersect_left(up_region, cut_value, localdim);
00892                 if (up_left_region==NULL) ok = 0;
00893             }
00894             else if(intersect==-1)
00895             /* intersect is -1 if no overlap */
00896             {
00897                 up_left_region = NULL;
00898             }
00899 
00900             intersect=Region_test_intersect_right(up_region, cut_value, localdim);
00901             if(intersect==-1)
00902             {
00903                 up_right_region = Region_create(up_region->_left, up_region->_right);
00904                 if (up_right_region==NULL) ok = 0;
00905             }
00906             else if(intersect==0)
00907             {
00908                 up_right_region = Region_create_intersect_right(up_region, cut_value, localdim);
00909                 if (up_right_region==NULL) ok = 0;
00910             }
00911             else if(intersect==+1)
00912             /* intersect is +1 if no overlap */
00913             {
00914                 up_right_region = NULL;
00915             }
00916         }
00917 
00918         if (ok)
00919             ok = KDTree_neighbor_search_pairs(tree, up_left, up_left_region, down_left, down_left_region, depth+1);
00920         if (ok)
00921             ok = KDTree_neighbor_search_pairs(tree, up_left, up_left_region, down_right, down_right_region, depth+1);
00922         if (ok)
00923             ok = KDTree_neighbor_search_pairs(tree, up_right, up_right_region, down_left, down_left_region, depth+1);
00924         if (ok)
00925             ok = KDTree_neighbor_search_pairs(tree, up_right, up_right_region, down_right, down_right_region, depth+1);
00926 
00927         Region_destroy(down_left_region);
00928         Region_destroy(down_right_region);
00929         Region_destroy(up_left_region);
00930         Region_destroy(up_right_region);
00931     }
00932     return ok;
00933 }
00934 
00935 static int KDTree_search_neighbors_in_bucket(struct KDTree* tree, struct Node *node)
00936 {
00937     long int i;
00938     int ok;
00939 
00940     for(i=node->_start; i<node->_end; i++)
00941     {
00942         struct DataPoint p1;
00943         long int j;
00944 
00945         p1=tree->_data_point_list[i];
00946 
00947         for (j=i+1; j<node->_end; j++)
00948         {
00949             struct DataPoint p2;
00950 
00951             p2=tree->_data_point_list[j];
00952 
00953             ok = KDTree_test_neighbors(tree, &p1, &p2);
00954             if (!ok) return 0;
00955         }
00956     }
00957     return 1;
00958 }
00959 
00960 static int KDTree__neighbor_search(struct KDTree* tree, struct Node *node, struct Region *region, int depth)
00961 {
00962     struct Node *left, *right;
00963     struct Region *left_region = NULL;
00964     struct Region *right_region = NULL;
00965     int localdim;
00966     int intersect;
00967     float cut_value;
00968     int ok = 1;
00969 
00970     localdim=depth%tree->dim;
00971 
00972     left=node->_left;
00973     right=node->_right;
00974 
00975     cut_value = node->_cut_value;
00976 
00977     /* planes of left and right nodes */
00978     intersect=Region_test_intersect_left(region, cut_value, localdim);
00979     if(intersect==1)
00980     {
00981         left_region = Region_create(region->_left, region->_right);
00982         if (!left_region) ok = 0;
00983     }
00984     else if(intersect==0)
00985     {
00986         left_region = Region_create_intersect_left(region, cut_value, localdim);
00987         if (!left_region) ok = 0;
00988     }
00989     else if(intersect==-1)
00990     /* intersect is -1 if no overlap */
00991     {
00992         left_region = NULL;
00993     }
00994 
00995     intersect=Region_test_intersect_right(region, cut_value, localdim);
00996     if(intersect==-1)
00997     {
00998         right_region = Region_create(region->_left, region->_right);
00999         if (!right_region) ok = 0;
01000     }
01001     else if(intersect==0)
01002     {
01003         right_region = Region_create_intersect_right(region, cut_value, localdim);
01004         if (!right_region) ok = 0;
01005     }
01006     else if(intersect==+1)
01007     /* intersect is +1 if no overlap */
01008     {
01009         right_region = NULL;
01010     }
01011 
01012     if (ok)
01013     {
01014         if (!Node_is_leaf(left))
01015         {
01016             /* search for pairs in this half plane */
01017             ok = KDTree__neighbor_search(tree, left, left_region, depth+1);
01018         }
01019         else
01020         {
01021             ok = KDTree_search_neighbors_in_bucket(tree, left);
01022         }
01023     }
01024     
01025     if (ok)
01026     {
01027         if (!Node_is_leaf(right))
01028         {
01029             /* search for pairs in this half plane */
01030             ok = KDTree__neighbor_search(tree, right, right_region, depth+1);
01031         }
01032         else
01033         {
01034             ok = KDTree_search_neighbors_in_bucket(tree, right);
01035         }
01036     }
01037 
01038     /* search for pairs between the half planes */
01039     if (ok)
01040     {
01041         ok = KDTree_neighbor_search_pairs(tree, left, left_region, right, right_region, depth+1);
01042     }
01043 
01044     /* cleanup */
01045     Region_destroy(left_region);
01046     Region_destroy(right_region);
01047 
01048     return ok;
01049 }
01050 
01051 int
01052 KDTree_neighbor_search(struct KDTree* tree, float neighbor_radius,
01053                        struct Neighbor** neighbors)
01054 {
01055     long int i;
01056     int ok;
01057     Region_dim=tree->dim;
01058 
01059     if(tree->_neighbor_list)
01060     {
01061         free(tree->_neighbor_list);
01062         tree->_neighbor_list = NULL;
01063     }
01064     tree->_neighbor_count=0;
01065     /* note the use of r^2 to avoid use of sqrt */
01066     tree->_neighbor_radius=neighbor_radius;
01067     tree->_neighbor_radius_sq=neighbor_radius*neighbor_radius;
01068 
01069     if (Node_is_leaf(tree->_root))
01070     {
01071         /* this is a boundary condition */
01072         /* bucket_size>nr of points */
01073         ok = KDTree_search_neighbors_in_bucket(tree, tree->_root);
01074     }
01075     else
01076     {
01077         /* "normal" situation */
01078         struct Region *region;
01079         /* start with [-INF, INF] */
01080         region= Region_create(NULL, NULL);
01081         if (!region) return 0;
01082         ok = KDTree__neighbor_search(tree, tree->_root, region, 0);
01083         Region_destroy(region);
01084     }
01085     if (!ok) return 0;
01086 
01087     *neighbors = NULL;
01088     for (i = 0; i < tree->_neighbor_count; i++)
01089     {
01090         struct Neighbor* neighbor = malloc(sizeof(struct Neighbor));
01091         if (!neighbor)
01092         {
01093             while(1)
01094             {
01095                 neighbor = *neighbors;
01096                 if (!neighbor) return 0;
01097                 *neighbors = neighbor->next;
01098                 free(neighbor);
01099             }
01100         }
01101         *neighbor = tree->_neighbor_list[i];
01102         neighbor->next = *neighbors;
01103         *neighbors = neighbor;
01104     }
01105 
01106     return 1;
01107 }
01108 
01109 int
01110 KDTree_neighbor_simple_search(struct KDTree* tree, float radius,
01111                               struct Neighbor** neighbors)
01112 {
01113     long int i;
01114     int ok = 1;
01115 
01116     Region_dim=tree->dim;
01117 
01118     tree->_neighbor_radius=radius;
01119     tree->_neighbor_radius_sq=radius*radius;
01120 
01121     tree->_neighbor_count=0;
01122     if (tree->_neighbor_list)
01123     {
01124         free(tree->_neighbor_list);
01125         tree->_neighbor_list = NULL;
01126     }
01127 
01128     DataPoint_sort(tree->_data_point_list, tree->_data_point_list_size, 0);
01129 
01130     for (i=0; i<tree->_data_point_list_size; i++)
01131     {
01132         float x1;
01133         long int j;
01134         struct DataPoint p1;
01135 
01136         p1=tree->_data_point_list[i];
01137         x1=p1._coord[0];
01138 
01139         for (j=i+1; j<tree->_data_point_list_size; j++)
01140         {
01141             struct DataPoint p2;
01142             float x2;
01143 
01144             p2=tree->_data_point_list[j];
01145             x2=p2._coord[0];
01146 
01147             if (fabs(x2-x1)<=radius)
01148             {
01149                 ok = KDTree_test_neighbors(tree, &p1, &p2);
01150                 if (!ok) break;
01151             }
01152             else
01153             {
01154                 break;
01155             }
01156         }
01157     }
01158 
01159     if (!ok) return 0;
01160 
01161     *neighbors = NULL;
01162 
01163     for (i = 0; i < tree->_neighbor_count; i++)
01164     {
01165         struct Neighbor* neighbor = malloc(sizeof(struct Neighbor));
01166         if (!neighbor)
01167         {
01168             while(1)
01169             {
01170                 neighbor = *neighbors;
01171                 if (!neighbor) return 0;
01172                 *neighbors = neighbor->next;
01173                 free(neighbor);
01174             }
01175         }
01176         *neighbor = tree->_neighbor_list[i];
01177         neighbor->next = *neighbors;
01178         *neighbors = neighbor;
01179     }
01180 
01181     return 1;
01182 }