Merge with trunk r39928
[blender-staging.git] / source / blender / blenlib / intern / BLI_kdtree.c
index c885e8c8a9c4ae5cf3828d9ebb8a82df1b829188..c906d507eb819e59105f32e4b62bf0450c1f59f0 100644 (file)
 #define SWAP(type, a, b) { type sw_ap; sw_ap=(a); (a)=(b); (b)=sw_ap; }
 #endif
 
 #define SWAP(type, a, b) { type sw_ap; sw_ap=(a); (a)=(b); (b)=sw_ap; }
 #endif
 
-typedef struct KDTreeNode {
-       struct KDTreeNode *left, *right;
-       float co[3], nor[3];
-       int index;
-       short d;
-} KDTreeNode;
-
 struct KDTree {
        KDTreeNode *nodes;
        int totnode;
 struct KDTree {
        KDTreeNode *nodes;
        int totnode;
@@ -450,3 +443,90 @@ int BLI_kdtree_range_search(KDTree *tree, float range, float *co, float *nor, KD
 
        return found;
 }
 
        return found;
 }
+
+
+static int add_in_range_thread_safe(KDTreeNearest *ptn, int found, int *totfoundstack, int index, float dist, float *co)
+{
+       KDTreeNearest *to;
+
+       if(found+1 > *totfoundstack)
+               return 0;
+
+       to = ptn + found;
+
+       to->index = index;
+       to->dist = sqrt(dist);
+       copy_v3_v3(to->co, co);
+       return 1;
+}
+int BLI_kdtree_range_search_thread_safe(KDTree *tree, float range, float *co, float *nor, KDTreeNearest *nearest, int limit)
+{
+       KDTreeNode *root, *node= NULL;
+       KDTreeNode **stack, *defaultstack[500];
+       KDTreeNearest *foundstack=nearest;
+       float range2 = range*range, dist2;
+       int totstack, cur=0, found=0;
+
+       if(!tree || !tree->root)
+               return 0;
+
+       stack= defaultstack;
+       totstack= 500;
+
+       root= tree->root;
+
+       if(co[root->d] + range < root->co[root->d]) {
+               if(root->left)
+                       stack[cur++]=root->left;
+       }
+       else if(co[root->d] - range > root->co[root->d]) {
+               if(root->right)
+                       stack[cur++]=root->right;
+       }
+       else {
+               dist2 = squared_distance(root->co, co, root->nor, nor);
+               if(dist2  <= range2)
+                       add_in_range_thread_safe(foundstack, found++, &limit, root->index, dist2, root->co);
+
+               if(root->left)
+                       stack[cur++]=root->left;
+               if(root->right)
+                       stack[cur++]=root->right;
+       }
+
+       while(cur--) {
+               node=stack[cur];
+
+               if(co[node->d] + range < node->co[node->d]) {
+                       if(node->left)
+                               stack[cur++]=node->left;
+               }
+               else if(co[node->d] - range > node->co[node->d]) {
+                       if(node->right)
+                               stack[cur++]=node->right;
+               }
+               else {
+                       dist2 = squared_distance(node->co, co, node->nor, nor);
+                       if(dist2 <= range2)
+                               if (!add_in_range_thread_safe(foundstack, found++, &limit, node->index, dist2, node->co))
+                                       break;
+
+                       if(node->left)
+                               stack[cur++]=node->left;
+                       if(node->right)
+                               stack[cur++]=node->right;
+               }
+
+               /* abort if running out of stack */
+               if(cur+3 > totstack)
+                       break;
+       }
+
+       if(stack != defaultstack)
+               MEM_freeN(stack);
+
+       if(found)
+               qsort(foundstack, found, sizeof(KDTreeNearest), range_compare);
+
+       return found;
+}
\ No newline at end of file