Reason of last commits kdopbvh tree is now on trunk.
[blender.git] / source / blender / blenlib / intern / BLI_kdopbvh.c
1 /**
2  *
3  * ***** BEGIN GPL LICENSE BLOCK *****
4  *
5  * This program is free software; you can redistribute it and/or
6  * modify it under the terms of the GNU General Public License
7  * as published by the Free Software Foundation; either version 2
8  * of the License, or (at your option) any later version.
9  *
10  * This program is distributed in the hope that it will be useful,
11  * but WITHOUT ANY WARRANTY; without even the implied warranty of
12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
13  * GNU General Public License for more details.
14  *
15  * You should have received a copy of the GNU General Public License
16  * along with this program; if not, write to the Free Software Foundation,
17  * Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
18  *
19  * The Original Code is Copyright (C) 2006 by NaN Holding BV.
20  * All rights reserved.
21  *
22  * The Original Code is: all of this file.
23  *
24  * Contributor(s): Daniel Genrich, Andre Pinto
25  *
26  * ***** END GPL LICENSE BLOCK *****
27  */
28
29 #include "math.h"
30 #include <stdio.h>
31 #include <stdlib.h> 
32 #include <string.h>
33
34 #include "MEM_guardedalloc.h"
35
36 #include "BKE_utildefines.h"
37
38 #include "BLI_kdopbvh.h"
39 #include "BLI_arithb.h"
40
41 #ifdef _OPENMP
42 #include <omp.h>
43 #endif
44
45 typedef struct BVHNode
46 {
47         struct BVHNode **children; // max 8 children
48         struct BVHNode *parent; // needed for bottom - top update
49         float *bv; // Bounding volume of all nodes, max 13 axis
50         int index; /* face, edge, vertex index */
51         char totnode; // how many nodes are used, used for speedup
52         char traversed;  // how many nodes already traversed until this level?
53         char main_axis;
54 } BVHNode;
55
56 struct BVHTree
57 {
58         BVHNode **nodes;
59         BVHNode *nodearray; /* pre-alloc branch nodes */
60         BVHNode **nodechild;    // pre-alloc childs for nodes
61         float   *nodebv;                // pre-alloc bounding-volumes for nodes
62         float   epsilon; /* epslion is used for inflation of the k-dop     */
63         int     totleaf; // leafs
64         int     totbranch;
65         char    tree_type; // type of tree (4 => quadtree)
66         char    axis; // kdop type (6 => OBB, 7 => AABB, ...)
67         char    start_axis, stop_axis; // KDOP_AXES array indices according to axis
68 };
69
70 typedef struct BVHOverlapData 
71 {  
72         BVHTree *tree1, *tree2; 
73         BVHTreeOverlap *overlap; 
74         int i, max_overlap; /* i is number of overlaps */
75 } BVHOverlapData;
76
77 typedef struct BVHNearestData
78 {
79         BVHTree *tree;
80         float   *co;
81         BVHTree_NearestPointCallback callback;
82         void    *userdata;
83         float proj[13];                 //coordinates projection over axis
84         BVHTreeNearest nearest;
85 } BVHNearestData;
86 ////////////////////////////////////////
87
88
89 ////////////////////////////////////////////////////////////////////////
90 // Bounding Volume Hierarchy Definition
91 // 
92 // Notes: From OBB until 26-DOP --> all bounding volumes possible, just choose type below
93 // Notes: You have to choose the type at compile time ITM
94 // Notes: You can choose the tree type --> binary, quad, octree, choose below
95 ////////////////////////////////////////////////////////////////////////
96
97 static float KDOP_AXES[13][3] =
98 { {1.0, 0, 0}, {0, 1.0, 0}, {0, 0, 1.0}, {1.0, 1.0, 1.0}, {1.0, -1.0, 1.0}, {1.0, 1.0, -1.0},
99 {1.0, -1.0, -1.0}, {1.0, 1.0, 0}, {1.0, 0, 1.0}, {0, 1.0, 1.0}, {1.0, -1.0, 0}, {1.0, 0, -1.0},
100 {0, 1.0, -1.0}
101 };
102
103 //////////////////////////////////////////////////////////////////////////////////////////////////////
104 // Introsort 
105 // with permission deriven from the following Java code:
106 // http://ralphunden.net/content/tutorials/a-guide-to-introsort/
107 // and he derived it from the SUN STL 
108 //////////////////////////////////////////////////////////////////////////////////////////////////////
109 static int size_threshold = 16;
110 /*
111 * Common methods for all algorithms
112 */
113 static int floor_lg(int a)
114 {
115         return (int)(floor(log(a)/log(2)));
116 }
117
118 /*
119 * Insertion sort algorithm
120 */
121 static void bvh_insertionsort(BVHNode **a, int lo, int hi, int axis)
122 {
123         int i,j;
124         BVHNode *t;
125         for (i=lo; i < hi; i++)
126         {
127                 j=i;
128                 t = a[i];
129                 while((j!=lo) && (t->bv[axis] < (a[j-1])->bv[axis]))
130                 {
131                         a[j] = a[j-1];
132                         j--;
133                 }
134                 a[j] = t;
135         }
136 }
137
138 static int bvh_partition(BVHNode **a, int lo, int hi, BVHNode * x, int axis)
139 {
140         int i=lo, j=hi;
141         while (1)
142         {
143                 while ((a[i])->bv[axis] < x->bv[axis]) i++;
144                 j--;
145                 while (x->bv[axis] < (a[j])->bv[axis]) j--;
146                 if(!(i < j))
147                         return i;
148                 SWAP( BVHNode* , a[i], a[j]);
149                 i++;
150         }
151 }
152
153 /*
154 * Heapsort algorithm
155 */
156 static void bvh_downheap(BVHNode **a, int i, int n, int lo, int axis)
157 {
158         BVHNode * d = a[lo+i-1];
159         int child;
160         while (i<=n/2)
161         {
162                 child = 2*i;
163                 if ((child < n) && ((a[lo+child-1])->bv[axis] < (a[lo+child])->bv[axis]))
164                 {
165                         child++;
166                 }
167                 if (!(d->bv[axis] < (a[lo+child-1])->bv[axis])) break;
168                 a[lo+i-1] = a[lo+child-1];
169                 i = child;
170         }
171         a[lo+i-1] = d;
172 }
173
174 static void bvh_heapsort(BVHNode **a, int lo, int hi, int axis)
175 {
176         int n = hi-lo, i;
177         for (i=n/2; i>=1; i=i-1)
178         {
179                 bvh_downheap(a, i,n,lo, axis);
180         }
181         for (i=n; i>1; i=i-1)
182         {
183                 SWAP(BVHNode*, a[lo],a[lo+i-1]);
184                 bvh_downheap(a, 1,i-1,lo, axis);
185         }
186 }
187
188 static BVHNode *bvh_medianof3(BVHNode **a, int lo, int mid, int hi, int axis) // returns Sortable
189 {
190         if ((a[mid])->bv[axis] < (a[lo])->bv[axis])
191         {
192                 if ((a[hi])->bv[axis] < (a[mid])->bv[axis])
193                         return a[mid];
194                 else
195                 {
196                         if ((a[hi])->bv[axis] < (a[lo])->bv[axis])
197                                 return a[hi];
198                         else
199                                 return a[lo];
200                 }
201         }
202         else
203         {
204                 if ((a[hi])->bv[axis] < (a[mid])->bv[axis])
205                 {
206                         if ((a[hi])->bv[axis] < (a[lo])->bv[axis])
207                                 return a[lo];
208                         else
209                                 return a[hi];
210                 }
211                 else
212                         return a[mid];
213         }
214 }
215 /*
216 * Quicksort algorithm modified for Introsort
217 */
218 static void bvh_introsort_loop (BVHNode **a, int lo, int hi, int depth_limit, int axis)
219 {
220         int p;
221
222         while (hi-lo > size_threshold)
223         {
224                 if (depth_limit == 0)
225                 {
226                         bvh_heapsort(a, lo, hi, axis);
227                         return;
228                 }
229                 depth_limit=depth_limit-1;
230                 p=bvh_partition(a, lo, hi, bvh_medianof3(a, lo, lo+((hi-lo)/2)+1, hi-1, axis), axis);
231                 bvh_introsort_loop(a, p, hi, depth_limit, axis);
232                 hi=p;
233         }
234 }
235
236 static void sort(BVHNode **a0, int begin, int end, int axis)
237 {
238         if (begin < end)
239         {
240                 BVHNode **a=a0;
241                 bvh_introsort_loop(a, begin, end, 2*floor_lg(end-begin), axis);
242                 bvh_insertionsort(a, begin, end, axis);
243         }
244 }
245 void sort_along_axis(BVHTree *tree, int start, int end, int axis)
246 {
247         sort(tree->nodes, start, end, axis);
248 }
249
250 //after a call to this function you can expect one of:
251 //      every node to left of a[n] are smaller or equal to it
252 //      every node to the right of a[n] are greater or equal to it
253 int partition_nth_element(BVHNode **a, int _begin, int _end, int n, int axis){        
254         int begin = _begin, end = _end, cut;        
255         while(end-begin > 3)        
256         {                            
257                 cut = bvh_partition(a, begin, end, bvh_medianof3(a, begin, (begin+end)/2, end-1, axis), axis );                 
258                 if(cut <= n)                        
259                         begin = cut;                
260                 else                        
261                         end = cut;        
262         }        
263         bvh_insertionsort(a, begin, end, axis);
264
265         return n;
266 }
267
268 //////////////////////////////////////////////////////////////////////////////////////////////////////
269
270 void BLI_bvhtree_free(BVHTree *tree)
271 {       
272         if(tree)
273         {
274                 MEM_freeN(tree->nodes);
275                 MEM_freeN(tree->nodearray);
276                 MEM_freeN(tree->nodebv);
277                 MEM_freeN(tree->nodechild);
278                 MEM_freeN(tree);
279         }
280 }
281
282 BVHTree *BLI_bvhtree_new(int maxsize, float epsilon, char tree_type, char axis)
283 {
284         BVHTree *tree;
285         int numbranches=0, i;
286         
287         // only support up to octree
288         if(tree_type > 8)
289                 return NULL;
290
291         tree = (BVHTree *)MEM_callocN(sizeof(BVHTree), "BVHTree");
292         
293         if(tree)
294         {
295                 tree->epsilon = epsilon;
296                 tree->tree_type = tree_type; 
297                 tree->axis = axis;
298                 
299                 if(axis == 26)
300                 {
301                         tree->start_axis = 0;
302                         tree->stop_axis = 13;
303                 }
304                 else if(axis == 18)
305                 {
306                         tree->start_axis = 7;
307                         tree->stop_axis = 13;
308                 }
309                 else if(axis == 14)
310                 {
311                         tree->start_axis = 0;
312                         tree->stop_axis = 7;
313                 }
314                 else if(axis == 8) // AABB
315                 {
316                         tree->start_axis = 0;
317                         tree->stop_axis = 4;
318                 }
319                 else if(axis == 6) // OBB
320                 {
321                         tree->start_axis = 0;
322                         tree->stop_axis = 3;
323                 }
324                 else
325                 {
326                         MEM_freeN(tree);
327                         return NULL;
328                 }
329
330
331                 // calculate max number of branches, our bvh kdop is "almost perfect"
332                 for(i = 1; i <= (int)ceil((float)((float)log(maxsize)/(float)log(tree_type))); i++)
333                         numbranches += (pow(tree_type, i) / tree_type);
334                 
335                 tree->nodes = (BVHNode **)MEM_callocN(sizeof(BVHNode *)*(numbranches+maxsize + tree_type), "BVHNodes");
336                 
337                 if(!tree->nodes)
338                 {
339                         MEM_freeN(tree);
340                         return NULL;
341                 }
342                 
343                 tree->nodebv = (float*)MEM_callocN(sizeof(float)* axis * (numbranches+maxsize + tree_type), "BVHNodeBV");
344                 if(!tree->nodebv)
345                 {
346                         MEM_freeN(tree->nodes);
347                         MEM_freeN(tree);
348                 }
349
350                 tree->nodechild = (BVHNode**)MEM_callocN(sizeof(BVHNode*) * tree_type * (numbranches+maxsize + tree_type), "BVHNodeBV");
351                 if(!tree->nodechild)
352                 {
353                         MEM_freeN(tree->nodebv);
354                         MEM_freeN(tree->nodes);
355                         MEM_freeN(tree);
356                 }
357
358                 tree->nodearray = (BVHNode *)MEM_callocN(sizeof(BVHNode)*(numbranches+maxsize + tree_type), "BVHNodeArray");
359                 
360                 if(!tree->nodearray)
361                 {
362                         MEM_freeN(tree->nodechild);
363                         MEM_freeN(tree->nodebv);
364                         MEM_freeN(tree->nodes);
365                         MEM_freeN(tree);
366                         return NULL;
367                 }
368
369                 //link the dynamic bv and child links
370                 for(i=0; i< numbranches+maxsize + tree_type; i++)
371                 {
372                         tree->nodearray[i].bv = tree->nodebv + i * axis;
373                         tree->nodearray[i].children = tree->nodechild + i * tree_type;
374                 }
375                 
376         }
377
378         return tree;
379 }
380
381
382 static void create_kdop_hull(BVHTree *tree, BVHNode *node, float *co, int numpoints, int moving)
383 {
384         float newminmax;
385         float *bv = node->bv;
386         int i, k;
387         
388         // don't init boudings for the moving case
389         if(!moving)
390         {
391                 for (i = tree->start_axis; i < tree->stop_axis; i++)
392                 {
393                         bv[2*i] = FLT_MAX;
394                         bv[2*i + 1] = -FLT_MAX;
395                 }
396         }
397         
398         for(k = 0; k < numpoints; k++)
399         {
400                 // for all Axes.
401                 for (i = tree->start_axis; i < tree->stop_axis; i++)
402                 {
403                         newminmax = INPR(&co[k * 3], KDOP_AXES[i]);
404                         if (newminmax < bv[2 * i])
405                                 bv[2 * i] = newminmax;
406                         if (newminmax > bv[(2 * i) + 1])
407                                 bv[(2 * i) + 1] = newminmax;
408                 }
409         }
410 }
411
412 // depends on the fact that the BVH's for each face is already build
413 static void refit_kdop_hull(BVHTree *tree, BVHNode *node, int start, int end)
414 {
415         float newmin,newmax;
416         int i, j;
417         float *bv = node->bv;
418         
419         for (i = tree->start_axis; i < tree->stop_axis; i++)
420         {
421                 bv[2*i] = FLT_MAX;
422                 bv[2*i + 1] = -FLT_MAX;
423         }
424
425         for (j = start; j < end; j++)
426         {
427                 // for all Axes.
428                 for (i = tree->start_axis; i < tree->stop_axis; i++)
429                 {
430                         newmin = tree->nodes[j]->bv[(2 * i)];   
431                         if ((newmin < bv[(2 * i)]))
432                                 bv[(2 * i)] = newmin;
433  
434                         newmax = tree->nodes[j]->bv[(2 * i) + 1];
435                         if ((newmax > bv[(2 * i) + 1]))
436                                 bv[(2 * i) + 1] = newmax;
437                 }
438         }
439 }
440
441 int BLI_bvhtree_insert(BVHTree *tree, int index, float *co, int numpoints)
442 {
443         BVHNode *node= NULL;
444         int i;
445         
446         // insert should only possible as long as tree->totbranch is 0
447         if(tree->totbranch > 0)
448                 return 0;
449         
450         if(tree->totleaf+1 >= MEM_allocN_len(tree->nodes))
451                 return 0;
452         
453         // TODO check if have enough nodes in array
454         
455         node = tree->nodes[tree->totleaf] = &(tree->nodearray[tree->totleaf]);
456         tree->totleaf++;
457         
458         create_kdop_hull(tree, node, co, numpoints, 0);
459         
460         // inflate the bv with some epsilon
461         for (i = tree->start_axis; i < tree->stop_axis; i++)
462         {
463                 node->bv[(2 * i)] -= tree->epsilon; // minimum 
464                 node->bv[(2 * i) + 1] += tree->epsilon; // maximum 
465         }
466
467         node->index= index;
468         
469         return 1;
470 }
471
472 // only supports x,y,z axis in the moment
473 // but we should use a plain and simple function here for speed sake
474 static char get_largest_axis(float *bv)
475 {
476         float middle_point[3];
477
478         middle_point[0] = (bv[1]) - (bv[0]); // x axis
479         middle_point[1] = (bv[3]) - (bv[2]); // y axis
480         middle_point[2] = (bv[5]) - (bv[4]); // z axis
481         if (middle_point[0] > middle_point[1]) 
482         {
483                 if (middle_point[0] > middle_point[2])
484                         return 1; // max x axis
485                 else
486                         return 5; // max z axis
487         }
488         else 
489         {
490                 if (middle_point[1] > middle_point[2])
491                         return 3; // max y axis
492                 else
493                         return 5; // max z axis
494         }
495 }
496
497 static void bvh_div_nodes(BVHTree *tree, BVHNode *node, int start, int end, char lastaxis)
498 {
499         char laxis;
500         int i, tend;
501         BVHNode *tnode;
502         int slice = (end-start+tree->tree_type-1)/tree->tree_type;      //division rounded up
503         
504         // Determine which axis to split along
505         laxis = get_largest_axis(node->bv);
506         
507         // split nodes along longest axis
508         for (i=0; start < end; start += slice, i++) //i counts the current child
509         {       
510                 tend = start + slice;
511                 
512                 if(tend > end) tend = end;
513                 
514                 if(tend-start == 1)     // ok, we have 1 left for this node
515                 {
516                         node->children[i] = tree->nodes[start];
517                         node->children[i]->parent = node;
518                 }
519                 else
520                 {
521                         tnode = node->children[i] = tree->nodes[tree->totleaf  + tree->totbranch] = &(tree->nodearray[tree->totbranch + tree->totleaf]);
522                         tree->totbranch++;
523                         tnode->parent = node;
524                         
525                         if(tend != end)
526                                 partition_nth_element(tree->nodes, start, end, tend, laxis);
527                         refit_kdop_hull(tree, tnode, start, tend);
528                         bvh_div_nodes(tree, tnode, start, tend, laxis);
529                 }
530                 node->totnode++;
531         }
532         
533         return;
534 }
535
536 static void verify_tree(BVHTree *tree)
537 {
538         int i, j, check = 0;
539         
540         // check the pointer list
541         for(i = 0; i < tree->totleaf; i++)
542         {
543                 if(tree->nodes[i]->parent == NULL)
544                         printf("Leaf has no parent: %d\n", i);
545                 else
546                 {
547                         for(j = 0; j < tree->tree_type; j++)
548                         {
549                                 if(tree->nodes[i]->parent->children[j] == tree->nodes[i])
550                                         check = 1;
551                         }
552                         if(!check)
553                         {
554                                 printf("Parent child relationship doesn't match: %d\n", i);
555                         }
556                         check = 0;
557                 }
558         }
559         
560         // check the leaf list
561         for(i = 0; i < tree->totleaf; i++)
562         {
563                 if(tree->nodearray[i].parent == NULL)
564                         printf("Leaf has no parent: %d\n", i);
565                 else
566                 {
567                         for(j = 0; j < tree->tree_type; j++)
568                         {
569                                 if(tree->nodearray[i].parent->children[j] == &tree->nodearray[i])
570                                         check = 1;
571                         }
572                         if(!check)
573                         {
574                                 printf("Parent child relationship doesn't match: %d\n", i);
575                         }
576                         check = 0;
577                 }
578         }
579         
580         printf("branches: %d, leafs: %d, total: %d\n", tree->totbranch, tree->totleaf, tree->totbranch + tree->totleaf);
581 }
582         
583 void BLI_bvhtree_balance(BVHTree *tree)
584 {
585         BVHNode *node;
586         
587         if(tree->totleaf == 0)
588                 return;
589         
590         // create root node
591         node = tree->nodes[tree->totleaf] = &(tree->nodearray[tree->totleaf]);
592         tree->totbranch++;
593         
594         // refit root bvh node
595         refit_kdop_hull(tree, tree->nodes[tree->totleaf], 0, tree->totleaf);
596         // create + balance tree
597         bvh_div_nodes(tree, tree->nodes[tree->totleaf], 0, tree->totleaf, 0);
598         
599         // verify_tree(tree);
600 }
601
602 // overlap - is it possbile for 2 bv's to collide ?
603 static int tree_overlap(BVHNode *node1, BVHNode *node2, int start_axis, int stop_axis)
604 {
605         float *bv1 = node1->bv;
606         float *bv2 = node2->bv;
607
608         float *bv1_end = bv1 + (stop_axis<<1);
609                 
610         bv1 += start_axis<<1;
611         bv2 += start_axis<<1;
612         
613         // test all axis if min + max overlap
614         for (; bv1 != bv1_end; bv1+=2, bv2+=2)
615         {
616                 if ((*(bv1) > *(bv2 + 1)) || (*(bv2) > *(bv1 + 1))) 
617                         return 0;
618         }
619         
620         return 1;
621 }
622
623 static void traverse(BVHOverlapData *data, BVHNode *node1, BVHNode *node2)
624 {
625         int j;
626         
627         if(tree_overlap(node1, node2, MIN2(data->tree1->start_axis, data->tree2->start_axis), MIN2(data->tree1->stop_axis, data->tree2->stop_axis)))
628         {
629                 // check if node1 is a leaf
630                 if(!node1->totnode)
631                 {
632                         // check if node2 is a leaf
633                         if(!node2->totnode)
634                         {
635                                 
636                                 if(node1 == node2)
637                                 {
638                                         return;
639                                 }
640                                         
641                                 if(data->i >= data->max_overlap)
642                                 {       
643                                         // try to make alloc'ed memory bigger
644                                         data->overlap = realloc(data->overlap, sizeof(BVHTreeOverlap)*data->max_overlap*2);
645                                         
646                                         if(!data->overlap)
647                                         {
648                                                 printf("Out of Memory in traverse\n");
649                                                 return;
650                                         }
651                                         data->max_overlap *= 2;
652                                 }
653                                 
654                                 // both leafs, insert overlap!
655                                 data->overlap[data->i].indexA = node1->index;
656                                 data->overlap[data->i].indexB = node2->index;
657
658                                 data->i++;
659                         }
660                         else
661                         {
662                                 for(j = 0; j < data->tree2->tree_type; j++)
663                                 {
664                                         if(node2->children[j])
665                                                 traverse(data, node1, node2->children[j]);
666                                 }
667                         }
668                 }
669                 else
670                 {
671                         
672                         for(j = 0; j < data->tree2->tree_type; j++)
673                         {
674                                 if(node1->children[j])
675                                         traverse(data, node1->children[j], node2);
676                         }
677                 }
678         }
679         return;
680 }
681
682 BVHTreeOverlap *BLI_bvhtree_overlap(BVHTree *tree1, BVHTree *tree2, int *result)
683 {
684         int j, total = 0;
685         BVHTreeOverlap *overlap = NULL, *to = NULL;
686         BVHOverlapData **data;
687         
688         // check for compatibility of both trees (can't compare 14-DOP with 18-DOP)
689         if((tree1->axis != tree2->axis) && ((tree1->axis == 14) || tree2->axis == 14))
690                 return 0;
691         
692         // fast check root nodes for collision before doing big splitting + traversal
693         if(!tree_overlap(tree1->nodes[tree1->totleaf], tree2->nodes[tree2->totleaf], MIN2(tree1->start_axis, tree2->start_axis), MIN2(tree1->stop_axis, tree2->stop_axis)))
694                 return 0;
695
696         data = MEM_callocN(sizeof(BVHOverlapData *)* tree1->tree_type, "BVHOverlapData_star");
697         
698         for(j = 0; j < tree1->tree_type; j++)
699         {
700                 data[j] = (BVHOverlapData *)MEM_callocN(sizeof(BVHOverlapData), "BVHOverlapData");
701                 
702                 // init BVHOverlapData
703                 data[j]->overlap = (BVHTreeOverlap *)malloc(sizeof(BVHTreeOverlap)*MAX2(tree1->totleaf, tree2->totleaf));
704                 data[j]->tree1 = tree1;
705                 data[j]->tree2 = tree2;
706                 data[j]->max_overlap = MAX2(tree1->totleaf, tree2->totleaf);
707                 data[j]->i = 0;
708         }
709
710 #pragma omp parallel for private(j) schedule(static)
711         for(j = 0; j < tree1->tree_type; j++)
712         {
713                 traverse(data[j], tree1->nodes[tree1->totleaf]->children[j], tree2->nodes[tree2->totleaf]);
714         }
715         
716         for(j = 0; j < tree1->tree_type; j++)
717                 total += data[j]->i;
718         
719         to = overlap = (BVHTreeOverlap *)MEM_callocN(sizeof(BVHTreeOverlap)*total, "BVHTreeOverlap");
720         
721         for(j = 0; j < tree1->tree_type; j++)
722         {
723                 memcpy(to, data[j]->overlap, data[j]->i*sizeof(BVHTreeOverlap));
724                 to+=data[j]->i;
725         }
726         
727         for(j = 0; j < tree1->tree_type; j++)
728         {
729                 free(data[j]->overlap);
730                 MEM_freeN(data[j]);
731         }
732         MEM_freeN(data);
733         
734         (*result) = total;
735         return overlap;
736 }
737
738
739 // bottom up update of bvh tree:
740 // join the 4 children here
741 static void node_join(BVHTree *tree, BVHNode *node)
742 {
743         int i, j;
744         
745         for (i = tree->start_axis; i < tree->stop_axis; i++)
746         {
747                 node->bv[2*i] = FLT_MAX;
748                 node->bv[2*i + 1] = -FLT_MAX;
749         }
750         
751         for (i = 0; i < tree->tree_type; i++)
752         {
753                 if (node->children[i]) 
754                 {
755                         for (j = tree->start_axis; j < tree->stop_axis; j++)
756                         {
757                                 // update minimum 
758                                 if (node->children[i]->bv[(2 * j)] < node->bv[(2 * j)]) 
759                                         node->bv[(2 * j)] = node->children[i]->bv[(2 * j)];
760                                 
761                                 // update maximum 
762                                 if (node->children[i]->bv[(2 * j) + 1] > node->bv[(2 * j) + 1])
763                                         node->bv[(2 * j) + 1] = node->children[i]->bv[(2 * j) + 1];
764                         }
765                 }
766                 else
767                         break;
768         }
769 }
770
771 // call before BLI_bvhtree_update_tree()
772 int BLI_bvhtree_update_node(BVHTree *tree, int index, float *co, float *co_moving, int numpoints)
773 {
774         BVHNode *node= NULL;
775         int i = 0;
776         
777         // check if index exists
778         if(index > tree->totleaf)
779                 return 0;
780         
781         node = tree->nodearray + index;
782         
783         create_kdop_hull(tree, node, co, numpoints, 0);
784         
785         if(co_moving)
786                 create_kdop_hull(tree, node, co_moving, numpoints, 1);
787         
788         // inflate the bv with some epsilon
789         for (i = tree->start_axis; i < tree->stop_axis; i++)
790         {
791                 node->bv[(2 * i)] -= tree->epsilon; // minimum 
792                 node->bv[(2 * i) + 1] += tree->epsilon; // maximum 
793         }
794         
795         return 1;
796 }
797
798 // call BLI_bvhtree_update_node() first for every node/point/triangle
799 void BLI_bvhtree_update_tree(BVHTree *tree)
800 {
801         BVHNode *leaf, *parent;
802         
803         // reset tree traversing flag
804         for (leaf = tree->nodearray + tree->totleaf; leaf != tree->nodearray + tree->totleaf + tree->totbranch; leaf++)
805                 leaf->traversed = 0;
806         
807         for (leaf = tree->nodearray; leaf != tree->nodearray + tree->totleaf; leaf++)
808         {
809                 for (parent = leaf->parent; parent; parent = parent->parent)
810                 {
811                         parent->traversed++;    // we tried to go up in hierarchy 
812                         if (parent->traversed < parent->totnode) 
813                                 break;  // we do not need to check further 
814                         else 
815                                 node_join(tree, parent);
816                 }
817         }
818 }
819
820 float BLI_bvhtree_getepsilon(BVHTree *tree)
821 {
822         return tree->epsilon;
823 }
824
825
826 //Nearest neighbour
827 static float squared_dist(const float *a, const float *b)
828 {
829         float tmp[3];
830         VECSUB(tmp, a, b);
831         return INPR(tmp, tmp);
832 }
833
834 static float calc_nearest_point(BVHNearestData *data, BVHNode *node, float *nearest)
835 {
836         int i;
837         const float *bv = node->bv;
838
839         //nearest on AABB hull
840         for(i=0; i != 3; i++, bv += 2)
841         {
842                 if(bv[0] > data->proj[i])
843                         nearest[i] = bv[0];
844                 else if(bv[1] < data->proj[i])
845                         nearest[i] = bv[1];
846                 else
847                         nearest[i] = data->proj[i];
848         }
849
850 /*
851         //nearest on a general hull
852         VECCOPY(nearest, data->co);
853         for(i = data->tree->start_axis; i != data->tree->stop_axis; i++, bv+=2)
854         {
855                 float proj = INPR( nearest, KDOP_AXES[i]);
856                 float dl = bv[0] - proj;
857                 float du = bv[1] - proj;
858
859                 if(dl > 0)
860                 {
861                         VECADDFAC(nearest, nearest, KDOP_AXES[i], dl);
862                 }
863                 else if(du < 0)
864                 {
865                         VECADDFAC(nearest, nearest, KDOP_AXES[i], du);
866                 }
867         }
868 */
869         return squared_dist(data->co, nearest);
870 }
871
872
873 static void dfs_find_nearest(BVHNearestData *data, BVHNode *node)
874 {
875         int i;
876         float nearest[3], sdist;
877
878         sdist = calc_nearest_point(data, node, nearest);
879         if(sdist >= data->nearest.dist) return;
880
881         if(node->totnode == 0)
882         {
883                 if(data->callback)
884                         sdist = data->callback(data->userdata , node->index, data->co, nearest);
885
886                 if(sdist >= data->nearest.dist) return;
887
888                 data->nearest.index     = node->index;
889                 VECCOPY(data->nearest.nearest, nearest);
890                 data->nearest.dist      = sdist;
891         }
892         else
893         {
894                 if(sdist < data->nearest.dist)
895                 {
896                         for(i=0; i != node->totnode; i++)
897                         {
898                                 dfs_find_nearest(data, node->children[i]);
899                         }
900                 }
901         }
902 }
903
904 int BLI_bvhtree_find_nearest(BVHTree *tree, float *co, BVHTreeNearest *nearest, BVHTree_NearestPointCallback callback, void *userdata)
905 {
906         int i;
907
908         BVHNearestData data;
909
910         //init data to search
911         data.tree = tree;
912         data.co = co;
913
914         data.callback = callback;
915         data.userdata = userdata;
916
917         for(i = data.tree->start_axis; i != data.tree->stop_axis; i++)
918         {
919                 data.proj[i] = INPR(data.co, KDOP_AXES[i]);
920         }
921
922         if(nearest)
923         {
924                 memcpy( &data.nearest , nearest, sizeof(*nearest) );
925         }
926         else
927         {
928                 data.nearest.index = -1;
929                 data.nearest.dist = FLT_MAX;
930         }
931
932         //dfs search
933         dfs_find_nearest(&data, tree->nodes[tree->totleaf] );
934
935         //copy back results
936         if(nearest)
937         {
938                 memcpy(nearest, &data.nearest, sizeof(*nearest));
939         }
940
941         return data.nearest.index;
942 }
943