svn merge -r 15392:15551 https://svn.blender.org/svnroot/bf-blender/trunk/blender
[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
86 } BVHNearestData;
87
88 typedef struct BVHRayCastData
89 {
90         BVHTree *tree;
91
92         BVHTree_RayCastCallback callback;
93         void    *userdata;
94
95
96         BVHTreeRay    ray;
97         float ray_dot_axis[13];
98
99         BVHTreeRayHit hit;
100 } BVHRayCastData;
101 ////////////////////////////////////////m
102
103
104 ////////////////////////////////////////////////////////////////////////
105 // Bounding Volume Hierarchy Definition
106 // 
107 // Notes: From OBB until 26-DOP --> all bounding volumes possible, just choose type below
108 // Notes: You have to choose the type at compile time ITM
109 // Notes: You can choose the tree type --> binary, quad, octree, choose below
110 ////////////////////////////////////////////////////////////////////////
111
112 static float KDOP_AXES[13][3] =
113 { {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},
114 {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},
115 {0, 1.0, -1.0}
116 };
117
118 //////////////////////////////////////////////////////////////////////////////////////////////////////
119 // Introsort 
120 // with permission deriven from the following Java code:
121 // http://ralphunden.net/content/tutorials/a-guide-to-introsort/
122 // and he derived it from the SUN STL 
123 //////////////////////////////////////////////////////////////////////////////////////////////////////
124 static int size_threshold = 16;
125 /*
126 * Common methods for all algorithms
127 */
128 static int floor_lg(int a)
129 {
130         return (int)(floor(log(a)/log(2)));
131 }
132
133 /*
134 * Insertion sort algorithm
135 */
136 static void bvh_insertionsort(BVHNode **a, int lo, int hi, int axis)
137 {
138         int i,j;
139         BVHNode *t;
140         for (i=lo; i < hi; i++)
141         {
142                 j=i;
143                 t = a[i];
144                 while((j!=lo) && (t->bv[axis] < (a[j-1])->bv[axis]))
145                 {
146                         a[j] = a[j-1];
147                         j--;
148                 }
149                 a[j] = t;
150         }
151 }
152
153 static int bvh_partition(BVHNode **a, int lo, int hi, BVHNode * x, int axis)
154 {
155         int i=lo, j=hi;
156         while (1)
157         {
158                 while ((a[i])->bv[axis] < x->bv[axis]) i++;
159                 j--;
160                 while (x->bv[axis] < (a[j])->bv[axis]) j--;
161                 if(!(i < j))
162                         return i;
163                 SWAP( BVHNode* , a[i], a[j]);
164                 i++;
165         }
166 }
167
168 /*
169 * Heapsort algorithm
170 */
171 static void bvh_downheap(BVHNode **a, int i, int n, int lo, int axis)
172 {
173         BVHNode * d = a[lo+i-1];
174         int child;
175         while (i<=n/2)
176         {
177                 child = 2*i;
178                 if ((child < n) && ((a[lo+child-1])->bv[axis] < (a[lo+child])->bv[axis]))
179                 {
180                         child++;
181                 }
182                 if (!(d->bv[axis] < (a[lo+child-1])->bv[axis])) break;
183                 a[lo+i-1] = a[lo+child-1];
184                 i = child;
185         }
186         a[lo+i-1] = d;
187 }
188
189 static void bvh_heapsort(BVHNode **a, int lo, int hi, int axis)
190 {
191         int n = hi-lo, i;
192         for (i=n/2; i>=1; i=i-1)
193         {
194                 bvh_downheap(a, i,n,lo, axis);
195         }
196         for (i=n; i>1; i=i-1)
197         {
198                 SWAP(BVHNode*, a[lo],a[lo+i-1]);
199                 bvh_downheap(a, 1,i-1,lo, axis);
200         }
201 }
202
203 static BVHNode *bvh_medianof3(BVHNode **a, int lo, int mid, int hi, int axis) // returns Sortable
204 {
205         if ((a[mid])->bv[axis] < (a[lo])->bv[axis])
206         {
207                 if ((a[hi])->bv[axis] < (a[mid])->bv[axis])
208                         return a[mid];
209                 else
210                 {
211                         if ((a[hi])->bv[axis] < (a[lo])->bv[axis])
212                                 return a[hi];
213                         else
214                                 return a[lo];
215                 }
216         }
217         else
218         {
219                 if ((a[hi])->bv[axis] < (a[mid])->bv[axis])
220                 {
221                         if ((a[hi])->bv[axis] < (a[lo])->bv[axis])
222                                 return a[lo];
223                         else
224                                 return a[hi];
225                 }
226                 else
227                         return a[mid];
228         }
229 }
230 /*
231 * Quicksort algorithm modified for Introsort
232 */
233 static void bvh_introsort_loop (BVHNode **a, int lo, int hi, int depth_limit, int axis)
234 {
235         int p;
236
237         while (hi-lo > size_threshold)
238         {
239                 if (depth_limit == 0)
240                 {
241                         bvh_heapsort(a, lo, hi, axis);
242                         return;
243                 }
244                 depth_limit=depth_limit-1;
245                 p=bvh_partition(a, lo, hi, bvh_medianof3(a, lo, lo+((hi-lo)/2)+1, hi-1, axis), axis);
246                 bvh_introsort_loop(a, p, hi, depth_limit, axis);
247                 hi=p;
248         }
249 }
250
251 static void sort(BVHNode **a0, int begin, int end, int axis)
252 {
253         if (begin < end)
254         {
255                 BVHNode **a=a0;
256                 bvh_introsort_loop(a, begin, end, 2*floor_lg(end-begin), axis);
257                 bvh_insertionsort(a, begin, end, axis);
258         }
259 }
260 void sort_along_axis(BVHTree *tree, int start, int end, int axis)
261 {
262         sort(tree->nodes, start, end, axis);
263 }
264
265 //after a call to this function you can expect one of:
266 //      every node to left of a[n] are smaller or equal to it
267 //      every node to the right of a[n] are greater or equal to it
268 int partition_nth_element(BVHNode **a, int _begin, int _end, int n, int axis){
269         int begin = _begin, end = _end, cut;
270         while(end-begin > 3)
271         {
272                 cut = bvh_partition(a, begin, end, bvh_medianof3(a, begin, (begin+end)/2, end-1, axis), axis );
273                 if(cut <= n)
274                         begin = cut;
275                 else
276                         end = cut;
277         }
278         bvh_insertionsort(a, begin, end, axis);
279
280         return n;
281 }
282
283 //////////////////////////////////////////////////////////////////////////////////////////////////////
284
285 void BLI_bvhtree_free(BVHTree *tree)
286 {       
287         if(tree)
288         {
289                 MEM_freeN(tree->nodes);
290                 MEM_freeN(tree->nodearray);
291                 MEM_freeN(tree->nodebv);
292                 MEM_freeN(tree->nodechild);
293                 MEM_freeN(tree);
294         }
295 }
296
297 BVHTree *BLI_bvhtree_new(int maxsize, float epsilon, char tree_type, char axis)
298 {
299         BVHTree *tree;
300         int numbranches=0, i;
301         
302         // theres not support for trees below binary-trees :P
303         if(tree_type < 2)
304                 return NULL;
305
306         tree = (BVHTree *)MEM_callocN(sizeof(BVHTree), "BVHTree");
307         
308         if(tree)
309         {
310                 tree->epsilon = epsilon;
311                 tree->tree_type = tree_type; 
312                 tree->axis = axis;
313                 
314                 if(axis == 26)
315                 {
316                         tree->start_axis = 0;
317                         tree->stop_axis = 13;
318                 }
319                 else if(axis == 18)
320                 {
321                         tree->start_axis = 7;
322                         tree->stop_axis = 13;
323                 }
324                 else if(axis == 14)
325                 {
326                         tree->start_axis = 0;
327                         tree->stop_axis = 7;
328                 }
329                 else if(axis == 8) // AABB
330                 {
331                         tree->start_axis = 0;
332                         tree->stop_axis = 4;
333                 }
334                 else if(axis == 6) // OBB
335                 {
336                         tree->start_axis = 0;
337                         tree->stop_axis = 3;
338                 }
339                 else
340                 {
341                         MEM_freeN(tree);
342                         return NULL;
343                 }
344
345
346                 // calculate max number of branches, our bvh kdop is "almost perfect"
347                 for(i = 1; i <= (int)ceil((float)((float)log(maxsize)/(float)log(tree_type))); i++)
348                         numbranches += (pow(tree_type, i) / tree_type);
349                 
350                 tree->nodes = (BVHNode **)MEM_callocN(sizeof(BVHNode *)*(numbranches+maxsize + tree_type), "BVHNodes");
351                 
352                 if(!tree->nodes)
353                 {
354                         MEM_freeN(tree);
355                         return NULL;
356                 }
357                 
358                 tree->nodebv = (float*)MEM_callocN(sizeof(float)* axis * (numbranches+maxsize + tree_type), "BVHNodeBV");
359                 if(!tree->nodebv)
360                 {
361                         MEM_freeN(tree->nodes);
362                         MEM_freeN(tree);
363                 }
364
365                 tree->nodechild = (BVHNode**)MEM_callocN(sizeof(BVHNode*) * tree_type * (numbranches+maxsize + tree_type), "BVHNodeBV");
366                 if(!tree->nodechild)
367                 {
368                         MEM_freeN(tree->nodebv);
369                         MEM_freeN(tree->nodes);
370                         MEM_freeN(tree);
371                 }
372
373                 tree->nodearray = (BVHNode *)MEM_callocN(sizeof(BVHNode)*(numbranches+maxsize + tree_type), "BVHNodeArray");
374                 
375                 if(!tree->nodearray)
376                 {
377                         MEM_freeN(tree->nodechild);
378                         MEM_freeN(tree->nodebv);
379                         MEM_freeN(tree->nodes);
380                         MEM_freeN(tree);
381                         return NULL;
382                 }
383
384                 //link the dynamic bv and child links
385                 for(i=0; i< numbranches+maxsize + tree_type; i++)
386                 {
387                         tree->nodearray[i].bv = tree->nodebv + i * axis;
388                         tree->nodearray[i].children = tree->nodechild + i * tree_type;
389                 }
390                 
391         }
392
393         return tree;
394 }
395
396
397 static void create_kdop_hull(BVHTree *tree, BVHNode *node, float *co, int numpoints, int moving)
398 {
399         float newminmax;
400         float *bv = node->bv;
401         int i, k;
402         
403         // don't init boudings for the moving case
404         if(!moving)
405         {
406                 for (i = tree->start_axis; i < tree->stop_axis; i++)
407                 {
408                         bv[2*i] = FLT_MAX;
409                         bv[2*i + 1] = -FLT_MAX;
410                 }
411         }
412         
413         for(k = 0; k < numpoints; k++)
414         {
415                 // for all Axes.
416                 for (i = tree->start_axis; i < tree->stop_axis; i++)
417                 {
418                         newminmax = INPR(&co[k * 3], KDOP_AXES[i]);
419                         if (newminmax < bv[2 * i])
420                                 bv[2 * i] = newminmax;
421                         if (newminmax > bv[(2 * i) + 1])
422                                 bv[(2 * i) + 1] = newminmax;
423                 }
424         }
425 }
426
427 // depends on the fact that the BVH's for each face is already build
428 static void refit_kdop_hull(BVHTree *tree, BVHNode *node, int start, int end)
429 {
430         float newmin,newmax;
431         int i, j;
432         float *bv = node->bv;
433
434         
435         for (i = tree->start_axis; i < tree->stop_axis; i++)
436         {
437                 bv[2*i] = FLT_MAX;
438                 bv[2*i + 1] = -FLT_MAX;
439         }
440
441         for (j = start; j < end; j++)
442         {
443 // for all Axes.
444                 for (i = tree->start_axis; i < tree->stop_axis; i++)
445                 {
446                         newmin = tree->nodes[j]->bv[(2 * i)];   
447                         if ((newmin < bv[(2 * i)]))
448                                 bv[(2 * i)] = newmin;
449  
450                         newmax = tree->nodes[j]->bv[(2 * i) + 1];
451                         if ((newmax > bv[(2 * i) + 1]))
452                                 bv[(2 * i) + 1] = newmax;
453                 }
454         }
455
456 }
457
458 int BLI_bvhtree_insert(BVHTree *tree, int index, float *co, int numpoints)
459 {
460         BVHNode *node= NULL;
461         int i;
462         
463         // insert should only possible as long as tree->totbranch is 0
464         if(tree->totbranch > 0)
465                 return 0;
466         
467         if(tree->totleaf+1 >= MEM_allocN_len(tree->nodes))
468                 return 0;
469         
470         // TODO check if have enough nodes in array
471         
472         node = tree->nodes[tree->totleaf] = &(tree->nodearray[tree->totleaf]);
473         tree->totleaf++;
474         
475         create_kdop_hull(tree, node, co, numpoints, 0);
476         
477         // inflate the bv with some epsilon
478         for (i = tree->start_axis; i < tree->stop_axis; i++)
479         {
480                 node->bv[(2 * i)] -= tree->epsilon; // minimum 
481                 node->bv[(2 * i) + 1] += tree->epsilon; // maximum 
482         }
483
484         node->index= index;
485         
486         return 1;
487 }
488
489 // only supports x,y,z axis in the moment
490 // but we should use a plain and simple function here for speed sake
491 static char get_largest_axis(float *bv)
492 {
493         float middle_point[3];
494
495         middle_point[0] = (bv[1]) - (bv[0]); // x axis
496         middle_point[1] = (bv[3]) - (bv[2]); // y axis
497         middle_point[2] = (bv[5]) - (bv[4]); // z axis
498         if (middle_point[0] > middle_point[1]) 
499         {
500                 if (middle_point[0] > middle_point[2])
501                         return 1; // max x axis
502                 else
503                         return 5; // max z axis
504         }
505         else 
506         {
507                 if (middle_point[1] > middle_point[2])
508                         return 3; // max y axis
509                 else
510                         return 5; // max z axis
511         }
512 }
513
514 static void bvh_div_nodes(BVHTree *tree, BVHNode *node, int start, int end, char lastaxis)
515 {
516         char laxis;
517         int i, tend;
518         BVHNode *tnode;
519         int slice = (end-start+tree->tree_type-1)/tree->tree_type;      //division rounded up
520         
521         // Determine which axis to split along
522         laxis = get_largest_axis(node->bv);
523         node->main_axis = laxis/2;
524         
525         // split nodes along longest axis
526         for (i=0; start < end; start += slice, i++) //i counts the current child
527         {       
528                 tend = start + slice;
529                 
530                 if(tend > end) tend = end;
531                 
532                 if(tend-start == 1)     // ok, we have 1 left for this node
533                 {
534                         node->children[i] = tree->nodes[start];
535                         node->children[i]->parent = node;
536                 }
537                 else
538                 {
539                         tnode = node->children[i] = tree->nodes[tree->totleaf  + tree->totbranch] = &(tree->nodearray[tree->totbranch + tree->totleaf]);
540                         tree->totbranch++;
541                         tnode->parent = node;
542                         
543                         if(tend != end)
544                                 partition_nth_element(tree->nodes, start, end, tend, laxis);
545                         refit_kdop_hull(tree, tnode, start, tend);
546                         bvh_div_nodes(tree, tnode, start, tend, laxis);
547                 }
548                 node->totnode++;
549         }
550         
551         return;
552 }
553
554 #if 0
555 static void verify_tree(BVHTree *tree)
556 {
557         int i, j, check = 0;
558         
559         // check the pointer list
560         for(i = 0; i < tree->totleaf; i++)
561         {
562                 if(tree->nodes[i]->parent == NULL)
563                         printf("Leaf has no parent: %d\n", i);
564                 else
565                 {
566                         for(j = 0; j < tree->tree_type; j++)
567                         {
568                                 if(tree->nodes[i]->parent->children[j] == tree->nodes[i])
569                                         check = 1;
570                         }
571                         if(!check)
572                         {
573                                 printf("Parent child relationship doesn't match: %d\n", i);
574                         }
575                         check = 0;
576                 }
577         }
578         
579         // check the leaf list
580         for(i = 0; i < tree->totleaf; i++)
581         {
582                 if(tree->nodearray[i].parent == NULL)
583                         printf("Leaf has no parent: %d\n", i);
584                 else
585                 {
586                         for(j = 0; j < tree->tree_type; j++)
587                         {
588                                 if(tree->nodearray[i].parent->children[j] == &tree->nodearray[i])
589                                         check = 1;
590                         }
591                         if(!check)
592                         {
593                                 printf("Parent child relationship doesn't match: %d\n", i);
594                         }
595                         check = 0;
596                 }
597         }
598         
599         printf("branches: %d, leafs: %d, total: %d\n", tree->totbranch, tree->totleaf, tree->totbranch + tree->totleaf);
600 }
601 #endif
602         
603 void BLI_bvhtree_balance(BVHTree *tree)
604 {
605         int i;
606         BVHNode *node;
607         
608         if(tree->totleaf == 0)
609                 return;
610         
611         // create root node
612         node = tree->nodes[tree->totleaf] = &(tree->nodearray[tree->totleaf]);
613         tree->totbranch++;
614         
615         // refit root bvh node
616         refit_kdop_hull(tree, tree->nodes[tree->totleaf], 0, tree->totleaf);
617         // create + balance tree
618         bvh_div_nodes(tree, tree->nodes[tree->totleaf], 0, tree->totleaf, 0);
619         
620         // verify_tree(tree);
621 }
622
623 // overlap - is it possbile for 2 bv's to collide ?
624 static int tree_overlap(BVHNode *node1, BVHNode *node2, int start_axis, int stop_axis)
625 {
626         float *bv1 = node1->bv;
627         float *bv2 = node2->bv;
628
629         float *bv1_end = bv1 + (stop_axis<<1);
630                 
631         bv1 += start_axis<<1;
632         bv2 += start_axis<<1;
633         
634         // test all axis if min + max overlap
635         for (; bv1 != bv1_end; bv1+=2, bv2+=2)
636         {
637                 if ((*(bv1) > *(bv2 + 1)) || (*(bv2) > *(bv1 + 1))) 
638                         return 0;
639         }
640         
641         return 1;
642 }
643
644 static void traverse(BVHOverlapData *data, BVHNode *node1, BVHNode *node2)
645 {
646         int j;
647         
648         if(tree_overlap(node1, node2, MIN2(data->tree1->start_axis, data->tree2->start_axis), MIN2(data->tree1->stop_axis, data->tree2->stop_axis)))
649         {
650                 // check if node1 is a leaf
651                 if(!node1->totnode)
652                 {
653                         // check if node2 is a leaf
654                         if(!node2->totnode)
655                         {
656                                 
657                                 if(node1 == node2)
658                                 {
659                                         return;
660                                 }
661                                         
662                                 if(data->i >= data->max_overlap)
663                                 {       
664                                         // try to make alloc'ed memory bigger
665                                         data->overlap = realloc(data->overlap, sizeof(BVHTreeOverlap)*data->max_overlap*2);
666                                         
667                                         if(!data->overlap)
668                                         {
669                                                 printf("Out of Memory in traverse\n");
670                                                 return;
671                                         }
672                                         data->max_overlap *= 2;
673                                 }
674                                 
675                                 // both leafs, insert overlap!
676                                 data->overlap[data->i].indexA = node1->index;
677                                 data->overlap[data->i].indexB = node2->index;
678
679                                 data->i++;
680                         }
681                         else
682                         {
683                                 for(j = 0; j < data->tree2->tree_type; j++)
684                                 {
685                                         if(node2->children[j])
686                                                 traverse(data, node1, node2->children[j]);
687                                 }
688                         }
689                 }
690                 else
691                 {
692                         
693                         for(j = 0; j < data->tree2->tree_type; j++)
694                         {
695                                 if(node1->children[j])
696                                         traverse(data, node1->children[j], node2);
697                         }
698                 }
699         }
700         return;
701 }
702
703 BVHTreeOverlap *BLI_bvhtree_overlap(BVHTree *tree1, BVHTree *tree2, int *result)
704 {
705         int j, total = 0;
706         BVHTreeOverlap *overlap = NULL, *to = NULL;
707         BVHOverlapData **data;
708         
709         // check for compatibility of both trees (can't compare 14-DOP with 18-DOP)
710         if((tree1->axis != tree2->axis) && ((tree1->axis == 14) || tree2->axis == 14))
711                 return 0;
712         
713         // fast check root nodes for collision before doing big splitting + traversal
714         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)))
715                 return 0;
716
717         data = MEM_callocN(sizeof(BVHOverlapData *)* tree1->tree_type, "BVHOverlapData_star");
718         
719         for(j = 0; j < tree1->tree_type; j++)
720         {
721                 data[j] = (BVHOverlapData *)MEM_callocN(sizeof(BVHOverlapData), "BVHOverlapData");
722                 
723                 // init BVHOverlapData
724                 data[j]->overlap = (BVHTreeOverlap *)malloc(sizeof(BVHTreeOverlap)*MAX2(tree1->totleaf, tree2->totleaf));
725                 data[j]->tree1 = tree1;
726                 data[j]->tree2 = tree2;
727                 data[j]->max_overlap = MAX2(tree1->totleaf, tree2->totleaf);
728                 data[j]->i = 0;
729         }
730
731 #pragma omp parallel for private(j) schedule(static)
732         for(j = 0; j < MIN2(tree1->tree_type, tree1->nodes[tree1->totleaf]->totnode); j++)
733         {
734                 traverse(data[j], tree1->nodes[tree1->totleaf]->children[j], tree2->nodes[tree2->totleaf]);
735         }
736         
737         for(j = 0; j < tree1->tree_type; j++)
738                 total += data[j]->i;
739         
740         to = overlap = (BVHTreeOverlap *)MEM_callocN(sizeof(BVHTreeOverlap)*total, "BVHTreeOverlap");
741         
742         for(j = 0; j < tree1->tree_type; j++)
743         {
744                 memcpy(to, data[j]->overlap, data[j]->i*sizeof(BVHTreeOverlap));
745                 to+=data[j]->i;
746         }
747         
748         for(j = 0; j < tree1->tree_type; j++)
749         {
750                 free(data[j]->overlap);
751                 MEM_freeN(data[j]);
752         }
753         MEM_freeN(data);
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 }
845
846
847
848 /*
849  * Nearest neighbour
850  */
851 static float squared_dist(const float *a, const float *b)
852 {
853         float tmp[3];
854         VECSUB(tmp, a, b);
855         return INPR(tmp, tmp);
856 }
857
858 static float calc_nearest_point(BVHNearestData *data, BVHNode *node, float *nearest)
859 {
860         int i;
861         const float *bv = node->bv;
862
863         //nearest on AABB hull
864         for(i=0; i != 3; i++, bv += 2)
865         {
866                 if(bv[0] > data->proj[i])
867                         nearest[i] = bv[0];
868                 else if(bv[1] < data->proj[i])
869                         nearest[i] = bv[1];
870                 else
871                         nearest[i] = data->proj[i];
872         }
873
874 /*
875         //nearest on a general hull
876         VECCOPY(nearest, data->co);
877         for(i = data->tree->start_axis; i != data->tree->stop_axis; i++, bv+=2)
878         {
879                 float proj = INPR( nearest, KDOP_AXES[i]);
880                 float dl = bv[0] - proj;
881                 float du = bv[1] - proj;
882
883                 if(dl > 0)
884                 {
885                         VECADDFAC(nearest, nearest, KDOP_AXES[i], dl);
886                 }
887                 else if(du < 0)
888                 {
889                         VECADDFAC(nearest, nearest, KDOP_AXES[i], du);
890                 }
891         }
892 */
893         return squared_dist(data->co, nearest);
894 }
895
896
897 static void dfs_find_nearest(BVHNearestData *data, BVHNode *node)
898 {
899         int i;
900         float nearest[3], sdist;
901
902         sdist = calc_nearest_point(data, node, nearest);
903         if(sdist >= data->nearest.dist) return;
904
905         if(node->totnode == 0)
906         {
907                 if(data->callback)
908                         sdist = data->callback(data->userdata , node->index, data->co, nearest);
909
910                 if(sdist >= data->nearest.dist) return;
911
912                 data->nearest.index     = node->index;
913                 VECCOPY(data->nearest.nearest, nearest);
914                 data->nearest.dist      = sdist;
915         }
916         else
917         {
918                 for(i=0; i != node->totnode; i++)
919                 {
920                         dfs_find_nearest(data, node->children[i]);
921                 }
922         }
923 }
924
925 int BLI_bvhtree_find_nearest(BVHTree *tree, float *co, BVHTreeNearest *nearest, BVHTree_NearestPointCallback callback, void *userdata)
926 {
927         int i;
928
929         BVHNearestData data;
930
931         //init data to search
932         data.tree = tree;
933         data.co = co;
934
935         data.callback = callback;
936         data.userdata = userdata;
937
938         for(i = data.tree->start_axis; i != data.tree->stop_axis; i++)
939         {
940                 data.proj[i] = INPR(data.co, KDOP_AXES[i]);
941         }
942
943         if(nearest)
944         {
945                 memcpy( &data.nearest , nearest, sizeof(*nearest) );
946         }
947         else
948         {
949                 data.nearest.index = -1;
950                 data.nearest.dist = FLT_MAX;
951         }
952
953         //dfs search
954         dfs_find_nearest(&data, tree->nodes[tree->totleaf] );
955
956         //copy back results
957         if(nearest)
958         {
959                 memcpy(nearest, &data.nearest, sizeof(*nearest));
960         }
961
962         return data.nearest.index;
963 }
964
965
966
967 /*
968  * Ray cast
969  */
970
971 static float ray_nearest_hit(BVHRayCastData *data, BVHNode *node)
972 {
973         int i;
974         const float *bv = node->bv;
975
976         float low = 0, upper = data->hit.dist;
977
978         for(i=0; i != 3; i++, bv += 2)
979         {
980                 if(data->ray_dot_axis[i] == 0.0f)
981                 {
982                         //axis aligned ray
983                         if(data->ray.origin[i] < bv[0]
984                         || data->ray.origin[i] > bv[1])
985                                 return FLT_MAX;
986                 }
987                 else
988                 {
989                         float ll = (bv[0] - data->ray.origin[i]) / data->ray_dot_axis[i];
990                         float lu = (bv[1] - data->ray.origin[i]) / data->ray_dot_axis[i];
991
992                         if(data->ray_dot_axis[i] > 0)
993                         {
994                                 if(ll > low)   low = ll;
995                                 if(lu < upper) upper = lu;
996                         }
997                         else
998                         {
999                                 if(lu > low)   low = lu;
1000                                 if(ll < upper) upper = ll;
1001                         }
1002         
1003                         if(low > upper) return FLT_MAX;
1004                 }
1005         }
1006         return low;
1007 }
1008
1009 static void dfs_raycast(BVHRayCastData *data, BVHNode *node)
1010 {
1011         int i;
1012         float dist;
1013
1014         dist = ray_nearest_hit(data, node);
1015
1016         if(dist >= data->hit.dist) return;
1017
1018         if(node->totnode == 0)
1019         {
1020                 if(data->callback)
1021                         dist = data->callback(data->userdata, node->index, &data->ray, &data->hit);
1022                 else
1023                 {
1024                         data->hit.index = node->index;
1025                         data->hit.dist  = dist;
1026                         VECADDFAC(data->hit.co, data->ray.origin, data->ray.direction, dist);
1027                 }
1028         }
1029         else
1030         {
1031                 //pick loop direction to dive into the tree (based on ray direction and split axis)
1032                 if(data->ray_dot_axis[ node->main_axis ] > 0)
1033                 {
1034                         for(i=0; i != node->totnode; i++)
1035                         {
1036                                 dfs_raycast(data, node->children[i]);
1037                         }
1038                 }
1039                 else
1040                 {
1041                         for(i=node->totnode-1; i >= 0; i--)
1042                         {
1043                                 dfs_raycast(data, node->children[i]);
1044                         }
1045                 }
1046         }
1047 }
1048
1049
1050
1051 int BLI_bvhtree_ray_cast(BVHTree *tree, float *co, float *dir, BVHTreeRayHit *hit, BVHTree_RayCastCallback callback, void *userdata)
1052 {
1053         int i;
1054         BVHRayCastData data;
1055
1056         data.tree = tree;
1057
1058         data.callback = callback;
1059         data.userdata = userdata;
1060
1061         VECCOPY(data.ray.origin,    co);
1062         VECCOPY(data.ray.direction, dir);
1063
1064         Normalize(data.ray.direction);
1065
1066         for(i=0; i<3; i++)
1067         {
1068                 data.ray_dot_axis[i] = INPR( data.ray.direction, KDOP_AXES[i]);
1069
1070                 if(fabs(data.ray_dot_axis[i]) < 1e-7)
1071                         data.ray_dot_axis[i] = 0.0;
1072         }
1073
1074
1075         if(hit)
1076                 memcpy( &data.hit, hit, sizeof(*hit) );
1077         else
1078         {
1079                 data.hit.index = -1;
1080                 data.hit.dist = FLT_MAX;
1081         }
1082
1083         dfs_raycast(&data, tree->nodes[tree->totleaf]);
1084
1085
1086         if(hit)
1087                 memcpy( hit, &data.hit, sizeof(*hit) );
1088
1089         return data.hit.index;
1090 }
1091