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