-== kdop ==-
[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 or equal to it
280 //      every node to the right of a[n] are greater or equal to it
281 int partition_nth_element(BVHNode **a, int _begin, int _end, int n, int axis){        
282         int begin = _begin, end = _end, cut;        
283         int i;         
284         while(end-begin > 3)        
285         {                            
286                 cut = bvh_partition(a, begin, end, bvh_medianof3(a, begin, (begin+end)/2, end-1, axis), axis );                 
287                 if(cut <= n)                        
288                         begin = cut;                
289                 else                        
290                         end = cut;        
291         }        
292         bvh_insertionsort(a, begin, end, axis);
293
294         return n;
295 }
296
297
298 //////////////////////////////////////////////////////////////////////////////////////////////////////
299
300 void BLI_bvhtree_free(BVHTree *tree)
301 {       
302         if(tree)
303         {
304                 MEM_freeN(tree->nodes);
305                 MEM_freeN(tree->nodearray);
306                 MEM_freeN(tree->nodebv);
307                 MEM_freeN(tree->nodechild);
308                 MEM_freeN(tree);
309         }
310 }
311
312 BVHTree *BLI_bvhtree_new(int maxsize, float epsilon, char tree_type, char axis)
313 {
314         BVHTree *tree;
315         int numbranches=0, i;
316         
317         // only support up to octree
318         if(tree_type > 8)
319                 return NULL;
320
321         tree = (BVHTree *)MEM_callocN(sizeof(BVHTree), "BVHTree");
322         
323         if(tree)
324         {
325                 tree->epsilon = epsilon;
326                 tree->tree_type = tree_type; 
327                 tree->axis = axis;
328                 
329                 if(axis == 26)
330                 {
331                         tree->start_axis = 0;
332                         tree->stop_axis = 13;
333                 }
334                 else if(axis == 18)
335                 {
336                         tree->start_axis = 7;
337                         tree->stop_axis = 13;
338                 }
339                 else if(axis == 14)
340                 {
341                         tree->start_axis = 0;
342                         tree->stop_axis = 7;
343                 }
344                 else if(axis == 8) // AABB
345                 {
346                         tree->start_axis = 0;
347                         tree->stop_axis = 4;
348                 }
349                 else if(axis == 6) // OBB
350                 {
351                         tree->start_axis = 0;
352                         tree->stop_axis = 3;
353                 }
354                 else
355                 {
356                         MEM_freeN(tree);
357                         return NULL;
358                 }
359
360
361                 // calculate max number of branches, our bvh kdop is "almost perfect"
362                 for(i = 1; i <= (int)ceil((float)((float)log(maxsize)/(float)log(tree_type))); i++)
363                         numbranches += (pow(tree_type, i) / tree_type);
364                 
365                 tree->nodes = (BVHNode **)MEM_callocN(sizeof(BVHNode *)*(numbranches+maxsize + tree_type), "BVHNodes");
366                 
367                 if(!tree->nodes)
368                 {
369                         MEM_freeN(tree);
370                         return NULL;
371                 }
372                 
373                 tree->nodebv = (float*)MEM_callocN(sizeof(float)* axis * (numbranches+maxsize + tree_type), "BVHNodeBV");
374                 if(!tree->nodebv)
375                 {
376                         MEM_freeN(tree->nodes);
377                         MEM_freeN(tree);
378                 }
379
380                 tree->nodechild = (BVHNode**)MEM_callocN(sizeof(BVHNode*) * tree_type * (numbranches+maxsize + tree_type), "BVHNodeBV");
381                 if(!tree->nodechild)
382                 {
383                         MEM_freeN(tree->nodebv);
384                         MEM_freeN(tree->nodes);
385                         MEM_freeN(tree);
386                 }
387
388                 tree->nodearray = (BVHNode *)MEM_callocN(sizeof(BVHNode)*(numbranches+maxsize + tree_type), "BVHNodeArray");
389                 
390                 if(!tree->nodearray)
391                 {
392                         MEM_freeN(tree->nodechild);
393                         MEM_freeN(tree->nodebv);
394                         MEM_freeN(tree->nodes);
395                         MEM_freeN(tree);
396                         return NULL;
397                 }
398
399                 //link the dynamic bv and child links
400                 for(i=0; i< numbranches+maxsize + tree_type; i++)
401                 {
402                         tree->nodearray[i].bv = tree->nodebv + i * axis;
403                         tree->nodearray[i].children = tree->nodechild + i * tree_type;
404                 }
405                 
406         }
407
408         return tree;
409 }
410
411
412 static void create_kdop_hull(BVHTree *tree, BVHNode *node, float *co, int numpoints, int moving)
413 {
414         float newminmax;
415         int i, k;
416         
417         // don't init boudings for the moving case
418         if(!moving)
419         {
420                 for (i = tree->start_axis; i < tree->stop_axis; i++)
421                 {
422                         node->bv[2*i] = FLT_MAX;
423                         node->bv[2*i + 1] = -FLT_MAX;
424                 }
425         }
426         
427         for(k = 0; k < numpoints; k++)
428         {
429                 // for all Axes.
430                 for (i = tree->start_axis; i < tree->stop_axis; i++)
431                 {
432                         newminmax = INPR(&co[k * 3], KDOP_AXES[i]);
433                         if (newminmax < node->bv[2 * i])
434                                 node->bv[2 * i] = newminmax;
435                         if (newminmax > node->bv[(2 * i) + 1])
436                                 node->bv[(2 * i) + 1] = newminmax;
437                 }
438         }
439 }
440
441 // depends on the fact that the BVH's for each face is already build
442 static void refit_kdop_hull(BVHTree *tree, BVHNode *node, int start, int end)
443 {
444         float newmin,newmax;
445         int i, j;
446         float *bv = node->bv;
447         
448         for (i = tree->start_axis; i < tree->stop_axis; i++)
449         {
450                 bv[2*i] = FLT_MAX;
451                 bv[2*i + 1] = -FLT_MAX;
452         }
453
454         for (j = start; j < end; j++)
455         {
456                 // for all Axes.
457                 for (i = tree->start_axis; i < tree->stop_axis; i++)
458                 {
459                         newmin = tree->nodes[j]->bv[(2 * i)];   
460                         if ((newmin < bv[(2 * i)]))
461                                 bv[(2 * i)] = newmin;
462  
463                         newmax = tree->nodes[j]->bv[(2 * i) + 1];
464                         if ((newmax > bv[(2 * i) + 1]))
465                                 bv[(2 * i) + 1] = newmax;
466                 }
467         }
468 }
469
470 int BLI_bvhtree_insert(BVHTree *tree, int index, float *co, int numpoints)
471 {
472         BVHNode *node= NULL;
473         int i;
474         
475         // insert should only possible as long as tree->totbranch is 0
476         if(tree->totbranch > 0)
477                 return 0;
478         
479         if(tree->totleaf+1 >= MEM_allocN_len(tree->nodes))
480                 return 0;
481         
482         // TODO check if have enough nodes in array
483         
484         node = tree->nodes[tree->totleaf] = &(tree->nodearray[tree->totleaf]);
485         tree->totleaf++;
486         
487         create_kdop_hull(tree, node, co, numpoints, 0);
488         
489         // inflate the bv with some epsilon
490         for (i = tree->start_axis; i < tree->stop_axis; i++)
491         {
492                 node->bv[(2 * i)] -= tree->epsilon; // minimum 
493                 node->bv[(2 * i) + 1] += tree->epsilon; // maximum 
494         }
495
496         node->index= index;
497         
498         return 1;
499 }
500
501 // only supports x,y,z axis in the moment
502 // but we should use a plain and simple function here for speed sake
503 static char get_largest_axis(float *bv)
504 {
505         float middle_point[3];
506
507         middle_point[0] = (bv[1]) - (bv[0]); // x axis
508         middle_point[1] = (bv[3]) - (bv[2]); // y axis
509         middle_point[2] = (bv[5]) - (bv[4]); // z axis
510         if (middle_point[0] > middle_point[1]) 
511         {
512                 if (middle_point[0] > middle_point[2])
513                         return 1; // max x axis
514                 else
515                         return 5; // max z axis
516         }
517         else 
518         {
519                 if (middle_point[1] > middle_point[2])
520                         return 3; // max y axis
521                 else
522                         return 5; // max z axis
523         }
524 }
525
526 static void bvh_div_nodes(BVHTree *tree, BVHNode *node, int start, int end, char lastaxis)
527 {
528         char laxis;
529         int i, tend;
530         BVHNode *tnode;
531         int slice = (end-start+tree->tree_type-1)/tree->tree_type;      //division rounded up
532         
533         // Determine which axis to split along
534         laxis = get_largest_axis(node->bv);
535         
536         // split nodes along longest axis
537         for (i=0; start < end; start += slice, i++) //i counts the current child
538         {       
539                 tend = start + slice;
540                 
541                 if(tend > end) tend = end;
542                 
543                 if(tend-start == 1)     // ok, we have 1 left for this node
544                 {
545                         node->children[i] = tree->nodes[start];
546                         node->children[i]->parent = node;
547                 }
548                 else
549                 {
550                         tnode = node->children[i] = tree->nodes[tree->totleaf  + tree->totbranch] = &(tree->nodearray[tree->totbranch + tree->totleaf]);
551                         tree->totbranch++;
552                         tnode->parent = node;
553                         
554                         if(tend != end)
555                                 partition_nth_element(tree->nodes, start, end, tend, laxis);
556                         refit_kdop_hull(tree, tnode, start, tend);
557                         bvh_div_nodes(tree, tnode, start, tend, laxis);
558                 }
559                 node->totnode++;
560         }
561         
562         return;
563 }
564
565 static void verify_tree(BVHTree *tree)
566 {
567         int i, j, check = 0;
568         
569         // check the pointer list
570         for(i = 0; i < tree->totleaf; i++)
571         {
572                 if(tree->nodes[i]->parent == NULL)
573                         printf("Leaf has no parent: %d\n", i);
574                 else
575                 {
576                         for(j = 0; j < tree->tree_type; j++)
577                         {
578                                 if(tree->nodes[i]->parent->children[j] == tree->nodes[i])
579                                         check = 1;
580                         }
581                         if(!check)
582                         {
583                                 printf("Parent child relationship doesn't match: %d\n", i);
584                         }
585                         check = 0;
586                 }
587         }
588         
589         // check the leaf list
590         for(i = 0; i < tree->totleaf; i++)
591         {
592                 if(tree->nodearray[i].parent == NULL)
593                         printf("Leaf has no parent: %d\n", i);
594                 else
595                 {
596                         for(j = 0; j < tree->tree_type; j++)
597                         {
598                                 if(tree->nodearray[i].parent->children[j] == &tree->nodearray[i])
599                                         check = 1;
600                         }
601                         if(!check)
602                         {
603                                 printf("Parent child relationship doesn't match: %d\n", i);
604                         }
605                         check = 0;
606                 }
607         }
608         
609         printf("branches: %d, leafs: %d, total: %d\n", tree->totbranch, tree->totleaf, tree->totbranch + tree->totleaf);
610 }
611         
612 void BLI_bvhtree_balance(BVHTree *tree)
613 {
614         BVHNode *node;
615         
616         if(tree->totleaf == 0)
617                 return;
618         
619         // create root node
620         node = tree->nodes[tree->totleaf] = &(tree->nodearray[tree->totleaf]);
621         tree->totbranch++;
622         
623         // refit root bvh node
624         refit_kdop_hull(tree, tree->nodes[tree->totleaf], 0, tree->totleaf);
625         // create + balance tree
626         bvh_div_nodes(tree, tree->nodes[tree->totleaf], 0, tree->totleaf, 0);
627         
628         // verify_tree(tree);
629 }
630
631 // overlap - is it possbile for 2 bv's to collide ?
632 static int tree_overlap(float *bv1, float *bv2, int start_axis, int stop_axis)
633 {
634         float *bv1_end = bv1 + (stop_axis<<1);
635                 
636         bv1 += start_axis<<1;
637         bv2 += start_axis<<1;
638         
639         // test all axis if min + max overlap
640         for (; bv1 != bv1_end; bv1+=2, bv2+=2)
641         {
642                 if ((*(bv1) > *(bv2 + 1)) || (*(bv2) > *(bv1 + 1))) 
643                         return 0;
644         }
645         
646         return 1;
647 }
648
649 static void traverse(BVHOverlapData *data, BVHNode *node1, BVHNode *node2)
650 {
651         int j;
652         
653         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)))
654         {
655                 // check if node1 is a leaf
656                 if(node1->index)
657                 {
658                         // check if node2 is a leaf
659                         if(node2->index)
660                         {
661                                 
662                                 if(node1 == node2)
663                                 {
664                                         return;
665                                 }
666                                         
667                                 if(data->i >= data->max_overlap)
668                                 {       
669                                         // try to make alloc'ed memory bigger
670                                         data->overlap = realloc(data->overlap, sizeof(BVHTreeOverlap)*data->max_overlap*2);
671                                         
672                                         if(!data->overlap)
673                                         {
674                                                 printf("Out of Memory in traverse\n");
675                                                 return;
676                                         }
677                                         data->max_overlap *= 2;
678                                 }
679                                 
680                                 // both leafs, insert overlap!
681                                 data->overlap[data->i].indexA = node1->index;
682                                 data->overlap[data->i].indexB = node2->index;
683
684                                 data->i++;
685                         }
686                         else
687                         {
688                                 for(j = 0; j < data->tree2->tree_type; j++)
689                                 {
690                                         if(node2->children[j])
691                                                 traverse(data, node1, node2->children[j]);
692                                 }
693                         }
694                 }
695                 else
696                 {
697                         
698                         for(j = 0; j < data->tree2->tree_type; j++)
699                         {
700                                 if(node1->children[j])
701                                         traverse(data, node1->children[j], node2);
702                         }
703                 }
704         }
705         return;
706 }
707
708 BVHTreeOverlap *BLI_bvhtree_overlap(BVHTree *tree1, BVHTree *tree2, int *result)
709 {
710         int j, total = 0;
711         BVHTreeOverlap *overlap = NULL, *to = NULL;
712         BVHOverlapData **data;
713         
714         // check for compatibility of both trees (can't compare 14-DOP with 18-DOP)
715         if((tree1->axis != tree2->axis) && ((tree1->axis == 14) || tree2->axis == 14))
716                 return 0;
717         
718         // fast check root nodes for collision before doing big splitting + traversal
719         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)))
720                 return 0;
721
722         data = MEM_callocN(sizeof(BVHOverlapData *)* tree1->tree_type, "BVHOverlapData_star");
723         
724         for(j = 0; j < tree1->tree_type; j++)
725         {
726                 data[j] = (BVHOverlapData *)MEM_callocN(sizeof(BVHOverlapData), "BVHOverlapData");
727                 
728                 // init BVHOverlapData
729                 data[j]->overlap = (BVHTreeOverlap *)malloc(sizeof(BVHTreeOverlap)*MAX2(tree1->totleaf, tree2->totleaf));
730                 data[j]->tree1 = tree1;
731                 data[j]->tree2 = tree2;
732                 data[j]->max_overlap = MAX2(tree1->totleaf, tree2->totleaf);
733                 data[j]->i = 0;
734         }
735
736 #pragma omp parallel for private(j) schedule(static)
737         for(j = 0; j < tree1->tree_type; j++)
738         {
739                 traverse(data[j], tree1->nodes[tree1->totleaf]->children[j], tree2->nodes[tree2->totleaf]);
740         }
741         
742         for(j = 0; j < tree1->tree_type; j++)
743                 total += data[j]->i;
744         
745         to = overlap = (BVHTreeOverlap *)MEM_callocN(sizeof(BVHTreeOverlap)*total, "BVHTreeOverlap");
746         
747         for(j = 0; j < tree1->tree_type; j++)
748         {
749                 memcpy(to, data[j]->overlap, data[j]->i*sizeof(BVHTreeOverlap));
750                 to+=data[j]->i;
751         }
752         
753         for(j = 0; j < tree1->tree_type; j++)
754         {
755                 free(data[j]->overlap);
756                 MEM_freeN(data[j]);
757         }
758         MEM_freeN(data);
759         
760         (*result) = total;
761         return overlap;
762 }
763
764
765 // bottom up update of bvh tree:
766 // join the 4 children here
767 static void node_join(BVHTree *tree, BVHNode *node)
768 {
769         int i, j;
770         
771         for (i = tree->start_axis; i < tree->stop_axis; i++)
772         {
773                 node->bv[2*i] = FLT_MAX;
774                 node->bv[2*i + 1] = -FLT_MAX;
775         }
776         
777         for (i = 0; i < tree->tree_type; i++)
778         {
779                 if (node->children[i]) 
780                 {
781                         for (j = tree->start_axis; j < tree->stop_axis; j++)
782                         {
783                                 // update minimum 
784                                 if (node->children[i]->bv[(2 * j)] < node->bv[(2 * j)]) 
785                                         node->bv[(2 * j)] = node->children[i]->bv[(2 * j)];
786                                 
787                                 // update maximum 
788                                 if (node->children[i]->bv[(2 * j) + 1] > node->bv[(2 * j) + 1])
789                                         node->bv[(2 * j) + 1] = node->children[i]->bv[(2 * j) + 1];
790                         }
791                 }
792                 else
793                         break;
794         }
795 }
796
797 // call before BLI_bvhtree_update_tree()
798 int BLI_bvhtree_update_node(BVHTree *tree, int index, float *co, float *co_moving, int numpoints)
799 {
800         BVHNode *node= NULL;
801         int i = 0;
802         
803         // check if index exists
804         if(index > tree->totleaf)
805                 return 0;
806         
807         node = tree->nodearray + index;
808         
809         create_kdop_hull(tree, node, co, numpoints, 0);
810         
811         if(co_moving)
812                 create_kdop_hull(tree, node, co_moving, numpoints, 1);
813         
814         // inflate the bv with some epsilon
815         for (i = tree->start_axis; i < tree->stop_axis; i++)
816         {
817                 node->bv[(2 * i)] -= tree->epsilon; // minimum 
818                 node->bv[(2 * i) + 1] += tree->epsilon; // maximum 
819         }
820         
821         return 1;
822 }
823
824 // call BLI_bvhtree_update_node() first for every node/point/triangle
825 void BLI_bvhtree_update_tree(BVHTree *tree)
826 {
827         BVHNode *leaf, *parent;
828         
829         // reset tree traversing flag
830         for (leaf = tree->nodearray + tree->totleaf; leaf != tree->nodearray + tree->totleaf + tree->totbranch; leaf++)
831                 leaf->traversed = 0;
832         
833         for (leaf = tree->nodearray; leaf != tree->nodearray + tree->totleaf; leaf++)
834         {
835                 for (parent = leaf->parent; parent; parent = parent->parent)
836                 {
837                         parent->traversed++;    // we tried to go up in hierarchy 
838                         if (parent->traversed < parent->totnode) 
839                                 break;  // we do not need to check further 
840                         else 
841                                 node_join(tree, parent);
842                 }
843         }
844 }
845
846 float BLI_bvhtree_getepsilon(BVHTree *tree)
847 {
848         return tree->epsilon;
849 }