BVH-KDOP update (merge from shrinkwrap branch): supports raytracing, nearest neighbou...
[blender-staging.git] / source / blender / blenlib / intern / BLI_kdopbvh.c
1 /**
2  *
3  * $Id$
4  *
5  * ***** BEGIN GPL LICENSE BLOCK *****
6  *
7  * This program is free software; you can redistribute it and/or
8  * modify it under the terms of the GNU General Public License
9  * as published by the Free Software Foundation; either version 2
10  * of the License, or (at your option) any later version.
11  *
12  * This program is distributed in the hope that it will be useful,
13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
15  * GNU General Public License for more details.
16  *
17  * You should have received a copy of the GNU General Public License
18  * along with this program; if not, write to the Free Software Foundation,
19  * Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
20  *
21  * The Original Code is Copyright (C) 2006 by NaN Holding BV.
22  * All rights reserved.
23  *
24  * The Original Code is: all of this file.
25  *
26  * Contributor(s): Daniel Genrich, Andre Pinto
27  *
28  * ***** END GPL LICENSE BLOCK *****
29  */
30
31 #include "math.h"
32 #include <stdio.h>
33 #include <stdlib.h>
34 #include <string.h>
35 #include <assert.h>
36
37 #include "MEM_guardedalloc.h"
38
39 #include "BKE_utildefines.h"
40
41 #include "BLI_kdopbvh.h"
42 #include "BLI_arithb.h"
43
44 #ifdef _OPENMP
45 #include <omp.h>
46 #endif
47
48 typedef struct BVHNode
49 {
50         struct BVHNode **children;      // max 8 children
51         struct BVHNode *parent; // needed for bottom - top update
52         float *bv;              // Bounding volume of all nodes, max 13 axis
53         int index;              // face, edge, vertex index
54         char totnode;   // how many nodes are used, used for speedup
55         char traversed;  // how many nodes already traversed until this level?
56         char main_axis;
57 } BVHNode;
58
59 struct BVHTree
60 {
61         BVHNode **nodes;
62         BVHNode *nodearray; /* pre-alloc branch nodes */
63         BVHNode **nodechild;    // pre-alloc childs for nodes
64         float   *nodebv;                // pre-alloc bounding-volumes for nodes
65         float   epsilon; /* epslion is used for inflation of the k-dop     */
66         int     totleaf; // leafs
67         int     totbranch;
68         char    tree_type; // type of tree (4 => quadtree)
69         char    axis; // kdop type (6 => OBB, 7 => AABB, ...)
70         char    start_axis, stop_axis; // KDOP_AXES array indices according to axis
71 };
72
73 typedef struct BVHOverlapData 
74 {  
75         BVHTree *tree1, *tree2; 
76         BVHTreeOverlap *overlap; 
77         int i, max_overlap; /* i is number of overlaps */
78 } BVHOverlapData;
79
80 typedef struct BVHNearestData
81 {
82         BVHTree *tree;
83         float   *co;
84         BVHTree_NearestPointCallback callback;
85         void    *userdata;
86         float proj[13];                 //coordinates projection over axis
87         BVHTreeNearest nearest;
88
89 } BVHNearestData;
90
91 typedef struct BVHRayCastData
92 {
93         BVHTree *tree;
94
95         BVHTree_RayCastCallback callback;
96         void    *userdata;
97
98
99         BVHTreeRay    ray;
100         float ray_dot_axis[13];
101
102         BVHTreeRayHit hit;
103 } BVHRayCastData;
104 ////////////////////////////////////////m
105
106
107 ////////////////////////////////////////////////////////////////////////
108 // Bounding Volume Hierarchy Definition
109 // 
110 // Notes: From OBB until 26-DOP --> all bounding volumes possible, just choose type below
111 // Notes: You have to choose the type at compile time ITM
112 // Notes: You can choose the tree type --> binary, quad, octree, choose below
113 ////////////////////////////////////////////////////////////////////////
114
115 static float KDOP_AXES[13][3] =
116 { {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},
117 {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},
118 {0, 1.0, -1.0}
119 };
120
121 //////////////////////////////////////////////////////////////////////////////////////////////////////
122 // Introsort 
123 // with permission deriven from the following Java code:
124 // http://ralphunden.net/content/tutorials/a-guide-to-introsort/
125 // and he derived it from the SUN STL 
126 //////////////////////////////////////////////////////////////////////////////////////////////////////
127 static int size_threshold = 16;
128 /*
129 * Common methods for all algorithms
130 */
131 static int floor_lg(int a)
132 {
133         return (int)(floor(log(a)/log(2)));
134 }
135
136 /*
137 * Insertion sort algorithm
138 */
139 static void bvh_insertionsort(BVHNode **a, int lo, int hi, int axis)
140 {
141         int i,j;
142         BVHNode *t;
143         for (i=lo; i < hi; i++)
144         {
145                 j=i;
146                 t = a[i];
147                 while((j!=lo) && (t->bv[axis] < (a[j-1])->bv[axis]))
148                 {
149                         a[j] = a[j-1];
150                         j--;
151                 }
152                 a[j] = t;
153         }
154 }
155
156 static int bvh_partition(BVHNode **a, int lo, int hi, BVHNode * x, int axis)
157 {
158         int i=lo, j=hi;
159         while (1)
160         {
161                 while ((a[i])->bv[axis] < x->bv[axis]) i++;
162                 j--;
163                 while (x->bv[axis] < (a[j])->bv[axis]) j--;
164                 if(!(i < j))
165                         return i;
166                 SWAP( BVHNode* , a[i], a[j]);
167                 i++;
168         }
169 }
170
171 /*
172 * Heapsort algorithm
173 */
174 static void bvh_downheap(BVHNode **a, int i, int n, int lo, int axis)
175 {
176         BVHNode * d = a[lo+i-1];
177         int child;
178         while (i<=n/2)
179         {
180                 child = 2*i;
181                 if ((child < n) && ((a[lo+child-1])->bv[axis] < (a[lo+child])->bv[axis]))
182                 {
183                         child++;
184                 }
185                 if (!(d->bv[axis] < (a[lo+child-1])->bv[axis])) break;
186                 a[lo+i-1] = a[lo+child-1];
187                 i = child;
188         }
189         a[lo+i-1] = d;
190 }
191
192 static void bvh_heapsort(BVHNode **a, int lo, int hi, int axis)
193 {
194         int n = hi-lo, i;
195         for (i=n/2; i>=1; i=i-1)
196         {
197                 bvh_downheap(a, i,n,lo, axis);
198         }
199         for (i=n; i>1; i=i-1)
200         {
201                 SWAP(BVHNode*, a[lo],a[lo+i-1]);
202                 bvh_downheap(a, 1,i-1,lo, axis);
203         }
204 }
205
206 static BVHNode *bvh_medianof3(BVHNode **a, int lo, int mid, int hi, int axis) // returns Sortable
207 {
208         if ((a[mid])->bv[axis] < (a[lo])->bv[axis])
209         {
210                 if ((a[hi])->bv[axis] < (a[mid])->bv[axis])
211                         return a[mid];
212                 else
213                 {
214                         if ((a[hi])->bv[axis] < (a[lo])->bv[axis])
215                                 return a[hi];
216                         else
217                                 return a[lo];
218                 }
219         }
220         else
221         {
222                 if ((a[hi])->bv[axis] < (a[mid])->bv[axis])
223                 {
224                         if ((a[hi])->bv[axis] < (a[lo])->bv[axis])
225                                 return a[lo];
226                         else
227                                 return a[hi];
228                 }
229                 else
230                         return a[mid];
231         }
232 }
233 /*
234 * Quicksort algorithm modified for Introsort
235 */
236 static void bvh_introsort_loop (BVHNode **a, int lo, int hi, int depth_limit, int axis)
237 {
238         int p;
239
240         while (hi-lo > size_threshold)
241         {
242                 if (depth_limit == 0)
243                 {
244                         bvh_heapsort(a, lo, hi, axis);
245                         return;
246                 }
247                 depth_limit=depth_limit-1;
248                 p=bvh_partition(a, lo, hi, bvh_medianof3(a, lo, lo+((hi-lo)/2)+1, hi-1, axis), axis);
249                 bvh_introsort_loop(a, p, hi, depth_limit, axis);
250                 hi=p;
251         }
252 }
253
254 static void sort(BVHNode **a0, int begin, int end, int axis)
255 {
256         if (begin < end)
257         {
258                 BVHNode **a=a0;
259                 bvh_introsort_loop(a, begin, end, 2*floor_lg(end-begin), axis);
260                 bvh_insertionsort(a, begin, end, axis);
261         }
262 }
263 void sort_along_axis(BVHTree *tree, int start, int end, int axis)
264 {
265         sort(tree->nodes, start, end, axis);
266 }
267
268 //after a call to this function you can expect one of:
269 //      every node to left of a[n] are smaller or equal to it
270 //      every node to the right of a[n] are greater or equal to it
271 int partition_nth_element(BVHNode **a, int _begin, int _end, int n, int axis){
272         int begin = _begin, end = _end, cut;
273         while(end-begin > 3)
274         {
275                 cut = bvh_partition(a, begin, end, bvh_medianof3(a, begin, (begin+end)/2, end-1, axis), axis );
276                 if(cut <= n)
277                         begin = cut;
278                 else
279                         end = cut;
280         }
281         bvh_insertionsort(a, begin, end, axis);
282
283         return n;
284 }
285
286 //////////////////////////////////////////////////////////////////////////////////////////////////////
287
288 void BLI_bvhtree_free(BVHTree *tree)
289 {       
290         if(tree)
291         {
292                 MEM_freeN(tree->nodes);
293                 MEM_freeN(tree->nodearray);
294                 MEM_freeN(tree->nodebv);
295                 MEM_freeN(tree->nodechild);
296                 MEM_freeN(tree);
297         }
298 }
299
300 // calculate max number of branches
301 int needed_branches(int tree_type, int leafs)
302 {
303 #if 1
304         //Worst case scenary  ( return max(0, leafs-tree_type)+1 )
305         if(leafs <= tree_type)
306                 return 1;
307         else
308                 return leafs-tree_type+1;
309
310 #else
311         //If our bvh kdop is "almost perfect"
312         //TODO i dont trust the float arithmetic in here (and I am not sure this formula is according to our splitting method)
313         int i, numbranches = 0;
314         for(i = 1; i <= (int)ceil((float)((float)log(leafs)/(float)log(tree_type))); i++)
315                 numbranches += (pow(tree_type, i) / tree_type);
316
317         return numbranches;
318 #endif
319 }
320                 
321
322 BVHTree *BLI_bvhtree_new(int maxsize, float epsilon, char tree_type, char axis)
323 {
324         BVHTree *tree;
325         int numnodes, i;
326         
327         // theres not support for trees below binary-trees :P
328         if(tree_type < 2)
329                 return NULL;
330
331         tree = (BVHTree *)MEM_callocN(sizeof(BVHTree), "BVHTree");
332         
333         if(tree)
334         {
335                 tree->epsilon = epsilon;
336                 tree->tree_type = tree_type; 
337                 tree->axis = axis;
338                 
339                 if(axis == 26)
340                 {
341                         tree->start_axis = 0;
342                         tree->stop_axis = 13;
343                 }
344                 else if(axis == 18)
345                 {
346                         tree->start_axis = 7;
347                         tree->stop_axis = 13;
348                 }
349                 else if(axis == 14)
350                 {
351                         tree->start_axis = 0;
352                         tree->stop_axis = 7;
353                 }
354                 else if(axis == 8) // AABB
355                 {
356                         tree->start_axis = 0;
357                         tree->stop_axis = 4;
358                 }
359                 else if(axis == 6) // OBB
360                 {
361                         tree->start_axis = 0;
362                         tree->stop_axis = 3;
363                 }
364                 else
365                 {
366                         MEM_freeN(tree);
367                         return NULL;
368                 }
369
370
371                 //Allocate arrays
372                 numnodes = maxsize + needed_branches(tree_type, maxsize) + tree_type;
373
374                 tree->nodes = (BVHNode **)MEM_callocN(sizeof(BVHNode *)*numnodes, "BVHNodes");
375                 
376                 if(!tree->nodes)
377                 {
378                         MEM_freeN(tree);
379                         return NULL;
380                 }
381                 
382                 tree->nodebv = (float*)MEM_callocN(sizeof(float)* axis * numnodes, "BVHNodeBV");
383                 if(!tree->nodebv)
384                 {
385                         MEM_freeN(tree->nodes);
386                         MEM_freeN(tree);
387                 }
388
389                 tree->nodechild = (BVHNode**)MEM_callocN(sizeof(BVHNode*) * tree_type * numnodes, "BVHNodeBV");
390                 if(!tree->nodechild)
391                 {
392                         MEM_freeN(tree->nodebv);
393                         MEM_freeN(tree->nodes);
394                         MEM_freeN(tree);
395                 }
396
397                 tree->nodearray = (BVHNode *)MEM_callocN(sizeof(BVHNode)* numnodes, "BVHNodeArray");
398                 
399                 if(!tree->nodearray)
400                 {
401                         MEM_freeN(tree->nodechild);
402                         MEM_freeN(tree->nodebv);
403                         MEM_freeN(tree->nodes);
404                         MEM_freeN(tree);
405                         return NULL;
406                 }
407
408                 //link the dynamic bv and child links
409                 for(i=0; i< numnodes; i++)
410                 {
411                         tree->nodearray[i].bv = tree->nodebv + i * axis;
412                         tree->nodearray[i].children = tree->nodechild + i * tree_type;
413                 }
414                 
415         }
416
417         return tree;
418 }
419
420
421 static void create_kdop_hull(BVHTree *tree, BVHNode *node, float *co, int numpoints, int moving)
422 {
423         float newminmax;
424         float *bv = node->bv;
425         int i, k;
426         
427         // don't init boudings for the moving case
428         if(!moving)
429         {
430                 for (i = tree->start_axis; i < tree->stop_axis; i++)
431                 {
432                         bv[2*i] = FLT_MAX;
433                         bv[2*i + 1] = -FLT_MAX;
434                 }
435         }
436         
437         for(k = 0; k < numpoints; k++)
438         {
439                 // for all Axes.
440                 for (i = tree->start_axis; i < tree->stop_axis; i++)
441                 {
442                         newminmax = INPR(&co[k * 3], KDOP_AXES[i]);
443                         if (newminmax < bv[2 * i])
444                                 bv[2 * i] = newminmax;
445                         if (newminmax > bv[(2 * i) + 1])
446                                 bv[(2 * i) + 1] = newminmax;
447                 }
448         }
449 }
450
451 // depends on the fact that the BVH's for each face is already build
452 static void refit_kdop_hull(BVHTree *tree, BVHNode *node, int start, int end)
453 {
454         float newmin,newmax;
455         int i, j;
456         float *bv = node->bv;
457
458         
459         for (i = tree->start_axis; i < tree->stop_axis; i++)
460         {
461                 bv[2*i] = FLT_MAX;
462                 bv[2*i + 1] = -FLT_MAX;
463         }
464
465         for (j = start; j < end; j++)
466         {
467 // for all Axes.
468                 for (i = tree->start_axis; i < tree->stop_axis; i++)
469                 {
470                         newmin = tree->nodes[j]->bv[(2 * i)];   
471                         if ((newmin < bv[(2 * i)]))
472                                 bv[(2 * i)] = newmin;
473  
474                         newmax = tree->nodes[j]->bv[(2 * i) + 1];
475                         if ((newmax > bv[(2 * i) + 1]))
476                                 bv[(2 * i) + 1] = newmax;
477                 }
478         }
479
480 }
481
482 int BLI_bvhtree_insert(BVHTree *tree, int index, float *co, int numpoints)
483 {
484         int i;
485         BVHNode *node = NULL;
486         
487         // insert should only possible as long as tree->totbranch is 0
488         if(tree->totbranch > 0)
489                 return 0;
490         
491         if(tree->totleaf+1 >= MEM_allocN_len(tree->nodes)/sizeof(*(tree->nodes)))
492                 return 0;
493         
494         // TODO check if have enough nodes in array
495         
496         node = tree->nodes[tree->totleaf] = &(tree->nodearray[tree->totleaf]);
497         tree->totleaf++;
498         
499         create_kdop_hull(tree, node, co, numpoints, 0);
500         node->index= index;
501         
502         // inflate the bv with some epsilon
503         for (i = tree->start_axis; i < tree->stop_axis; i++)
504         {
505                 node->bv[(2 * i)] -= tree->epsilon; // minimum 
506                 node->bv[(2 * i) + 1] += tree->epsilon; // maximum 
507         }
508
509         return 1;
510 }
511
512 // only supports x,y,z axis in the moment
513 // but we should use a plain and simple function here for speed sake
514 static char get_largest_axis(float *bv)
515 {
516         float middle_point[3];
517
518         middle_point[0] = (bv[1]) - (bv[0]); // x axis
519         middle_point[1] = (bv[3]) - (bv[2]); // y axis
520         middle_point[2] = (bv[5]) - (bv[4]); // z axis
521         if (middle_point[0] > middle_point[1]) 
522         {
523                 if (middle_point[0] > middle_point[2])
524                         return 1; // max x axis
525                 else
526                         return 5; // max z axis
527         }
528         else 
529         {
530                 if (middle_point[1] > middle_point[2])
531                         return 3; // max y axis
532                 else
533                         return 5; // max z axis
534         }
535 }
536
537 static void bvh_div_nodes(BVHTree *tree, BVHNode *node, int start, int end, int free_node_index)
538 {
539         int i;
540
541         const char laxis = get_largest_axis(node->bv);  //determine longest axis to split along
542         const int  slice = (end-start)/tree->tree_type; //division rounded down
543         const int  rest  = (end-start)%tree->tree_type; //remainder of division
544         
545         assert( node->totnode == 0 );
546
547         node->main_axis = laxis/2;
548         
549         // split nodes along longest axis
550         for (i=0; start < end; node->totnode = ++i) //i counts the current child
551         {       
552                 int tend = start + slice + (i < rest ? 1 : 0);
553                 
554                 assert( tend <= end);
555                 
556                 if(tend-start == 1)     // ok, we have 1 left for this node
557                 {
558                         node->children[i] = tree->nodes[start];
559                         node->children[i]->parent = node;
560                 }
561                 else
562                 {
563                         BVHNode *tnode = node->children[i] = tree->nodes[free_node_index] = &(tree->nodearray[free_node_index]);
564                         tnode->parent = node;
565                         
566                         if(tend != end)
567                                 partition_nth_element(tree->nodes, start, end, tend, laxis);
568
569                         refit_kdop_hull(tree, tnode, start, tend);
570
571                         bvh_div_nodes(tree, tnode, start, tend, free_node_index+1);
572                         free_node_index += needed_branches(tree->tree_type, tend-start);
573                 }
574                 start = tend;
575         }
576         
577         return;
578 }
579
580 static void omp_bvh_div_nodes(BVHTree *tree, BVHNode *node, int start, int end, int free_node_index)
581 {
582         int i;
583
584         const char laxis = get_largest_axis(node->bv);  //determine longest axis to split along
585         const int  slice = (end-start)/tree->tree_type; //division rounded down
586         const int  rest  = (end-start)%tree->tree_type; //remainder of division
587
588         int omp_data_start[tree->tree_type];
589         int omp_data_end  [tree->tree_type];
590         int omp_data_index[tree->tree_type];
591         
592         assert( node->totnode == 0 );
593
594         node->main_axis = laxis/2;      
595
596         // split nodes along longest axis
597         for (i=0; start < end; node->totnode = ++i) //i counts the current child
598         {       
599                 //Split the rest from left to right (TODO: this doenst makes an optimal tree)
600                 int tend = start + slice + (i < rest ? 1 : 0);
601                 
602                 assert( tend <= end);
603                 
604                 //save data for later OMP
605                 omp_data_start[i] = start;
606                 omp_data_end  [i] = tend;
607                 omp_data_index[i] = free_node_index;
608
609                 if(tend-start == 1)
610                 {
611                         node->children[i] = tree->nodes[start];
612                         node->children[i]->parent = node;
613                 }
614                 else
615                 {
616                         node->children[i] = tree->nodes[free_node_index] = &(tree->nodearray[free_node_index]);
617                         node->children[i]->parent = node;
618
619                         if(tend != end)
620                                 partition_nth_element(tree->nodes, start, end, tend, laxis);
621
622                         free_node_index += needed_branches(tree->tree_type, tend-start);
623                 }
624
625                 start = tend;
626         }
627
628 #pragma omp parallel for private(i) schedule(static)
629         for( i = 0; i < node->totnode; i++)
630         {
631                 if(omp_data_end[i]-omp_data_start[i] > 1)
632                 {
633                         BVHNode *tnode = node->children[i];
634                         refit_kdop_hull(tree, tnode, omp_data_start[i], omp_data_end[i]);
635                         bvh_div_nodes  (tree, tnode, omp_data_start[i], omp_data_end[i], omp_data_index[i]+1);
636                 }
637         }
638         
639         return;
640 }
641
642
643 static void print_tree(BVHTree *tree, BVHNode *node, int depth)
644 {
645         int i;
646         for(i=0; i<depth; i++) printf(" ");
647         printf(" - %d (%d): ", node->index, node - tree->nodearray);
648         for(i=2*tree->start_axis; i<2*tree->stop_axis; i++)
649                 printf("%.3f ", node->bv[i]);
650         printf("\n");
651
652         for(i=0; i<tree->tree_type; i++)
653                 if(node->children[i])
654                         print_tree(tree, node->children[i], depth+1);
655 }
656
657 #if 0
658
659 static void verify_tree(BVHTree *tree)
660 {
661         int i, j, check = 0;
662         
663         // check the pointer list
664         for(i = 0; i < tree->totleaf; i++)
665 {
666                 if(tree->nodes[i]->parent == NULL)
667                         printf("Leaf has no parent: %d\n", i);
668                                                else
669 {
670                                                for(j = 0; j < tree->tree_type; j++)
671 {
672                                                if(tree->nodes[i]->parent->children[j] == tree->nodes[i])
673                                                check = 1;
674 }
675                                                if(!check)
676 {
677                                                printf("Parent child relationship doesn't match: %d\n", i);
678 }
679                                                check = 0;
680 }
681 }
682         
683         // check the leaf list
684                                                for(i = 0; i < tree->totleaf; i++)
685 {
686                                                if(tree->nodearray[i].parent == NULL)
687                                                printf("Leaf has no parent: %d\n", i);
688                                                else
689 {
690                                                for(j = 0; j < tree->tree_type; j++)
691 {
692                                                if(tree->nodearray[i].parent->children[j] == &tree->nodearray[i])
693                                                check = 1;
694 }
695                                                if(!check)
696 {
697                                                printf("Parent child relationship doesn't match: %d\n", i);
698 }
699                                                check = 0;
700 }
701 }
702         
703                                                printf("branches: %d, leafs: %d, total: %d\n", tree->totbranch, tree->totleaf, tree->totbranch + tree->totleaf);
704 }
705 #endif
706
707 //Helper data and structures to build generalized implicit trees
708 //This code can be easily reduced
709 typedef struct BVHBuildHelper
710 {
711         int tree_type; //
712         int totleafs; //
713
714         int leafs_per_child  [32]; //Min number of leafs that are archievable from a node at depth N
715         int branches_on_level[32]; //Number of nodes at depth N (tree_type^N)
716
717         int remain_leafs; //Number of leafs that are placed on the level that is not 100% filled
718
719 } BVHBuildHelper;
720
721 static void build_implicit_tree_helper(BVHTree *tree, BVHBuildHelper *data)
722 {
723         int depth = 0;
724         int remain;
725         int nnodes;
726
727         data->totleafs = tree->totleaf;
728         data->tree_type= tree->tree_type;
729
730         //Calculate the smallest tree_type^n such that tree_type^n >= num_leafs
731         for(
732             data->leafs_per_child[0] = 1;
733                    data->leafs_per_child[0] <  data->totleafs;
734                    data->leafs_per_child[0] *= data->tree_type
735            );
736
737         data->branches_on_level[0] = 1;
738
739         //We could stop the loop first (but I am lazy to find out when)
740         for(depth = 1; depth < 32; depth++)
741         {
742                 data->branches_on_level[depth] = data->branches_on_level[depth-1] * data->tree_type;
743                 data->leafs_per_child  [depth] = data->leafs_per_child  [depth-1] / data->tree_type;
744         }
745
746         remain = data->totleafs - data->leafs_per_child[1];
747         nnodes = (remain + data->tree_type - 2) / (data->tree_type - 1);
748         data->remain_leafs = remain + nnodes;
749 }
750
751 // return the min index of all the leafs archivable with the given branch
752 static int implicit_leafs_index(BVHBuildHelper *data, int depth, int child_index)
753 {
754         int min_leaf_index = child_index * data->leafs_per_child[depth-1];
755         if(min_leaf_index <= data->remain_leafs)
756                 return min_leaf_index;
757         else if(data->leafs_per_child[depth])
758                 return data->totleafs - (data->branches_on_level[depth-1] - child_index) * data->leafs_per_child[depth];
759         else
760                 return data->remain_leafs;
761 }
762
763 //WARNING: Beautiful/tricky code starts here :P
764 //Generalized implicit trees
765 static void non_recursive_bvh_div_nodes(BVHTree *tree)
766 {
767         int i;
768
769         const int tree_type   = tree->tree_type;
770         const int tree_offset = 2 - tree->tree_type; //this value is 0 (on binary trees) and negative on the others
771         const int num_leafs   = tree->totleaf;
772         const int num_branches= MAX2(1, (num_leafs + tree_type - 3) / (tree_type-1) );
773
774         BVHNode*  branches_array = tree->nodearray + tree->totleaf - 1; // This code uses 1 index arrays
775         BVHNode** leafs_array    = tree->nodes;
776
777         BVHBuildHelper data;
778         int depth  = 0;
779
780         build_implicit_tree_helper(tree, &data);
781
782         //YAY this could be 1 loop.. but had to split in 2 to remove OMP dependencies
783         for(i=1; i <= num_branches; i = i*tree_type + tree_offset)
784         {
785                 const int first_of_next_level = i*tree_type + tree_offset;
786                 const int  end_j = MIN2(first_of_next_level, num_branches + 1); //index of last branch on this level
787                 int j;
788
789                 depth++;
790
791 #pragma omp parallel for private(j) schedule(static)
792                 for(j = i; j < end_j; j++)
793                 {
794                         int k;
795                         const int parent_level_index= j-i;
796                         BVHNode* parent = branches_array + j;
797                         char split_axis;
798
799                         int parent_leafs_begin = implicit_leafs_index(&data, depth, parent_level_index);
800                         int parent_leafs_end   = implicit_leafs_index(&data, depth, parent_level_index+1);
801
802                         //split_axis = (depth*2 % 6); //use this instead of the 2 following lines for XYZ splitting
803
804                         refit_kdop_hull(tree, parent, parent_leafs_begin, parent_leafs_end);
805                         split_axis = get_largest_axis(parent->bv);
806
807                         parent->main_axis = split_axis / 2;
808
809                         for(k = 0; k < tree_type; k++)
810                         {
811                                 int child_index = j * tree_type + tree_offset + k;
812                                 int child_level_index = child_index - first_of_next_level; //child level index
813
814                                 int child_leafs_begin = implicit_leafs_index(&data, depth+1, child_level_index);
815                                 int child_leafs_end   = implicit_leafs_index(&data, depth+1, child_level_index+1);
816
817                                 assert( k != 0 || child_leafs_begin == parent_leafs_begin);
818
819                                 if(child_leafs_end - child_leafs_begin > 1)
820                                 {
821                                         parent->children[k] = branches_array + child_index;
822                                         parent->children[k]->parent = parent;
823
824 /*
825                                         printf("Add child %d (%d) to branch %d\n",
826                                         branches_array  + child_index - tree->nodearray,
827                                         branches_array[ child_index ].index,
828                                         parent - tree->nodearray
829                                                 );
830 */
831
832                                         partition_nth_element(leafs_array, child_leafs_begin, parent_leafs_end, child_leafs_end, split_axis);
833                                 }
834                                 else if(child_leafs_end - child_leafs_begin == 1)
835                                 {
836 /*
837                                         printf("Add child %d (%d) to branch %d\n",
838                                         leafs_array[ child_leafs_begin ] - tree->nodearray,
839                                         leafs_array[ child_leafs_begin ]->index,
840                                         parent - tree->nodearray
841                                                 );
842 */
843                                         parent->children[k] = leafs_array[ child_leafs_begin ];
844                                         parent->children[k]->parent = parent;
845                                 }
846                                 else
847                                 {
848                                         parent->children[k] = NULL;
849                                         break;
850                                 }
851                                 parent->totnode = k+1;
852                         }
853                 }
854         }
855
856
857         for(i = 0; i<num_branches; i++)
858                 tree->nodes[tree->totleaf + i] = branches_array + 1 + i;
859
860         tree->totbranch = num_branches;
861
862 //      BLI_bvhtree_update_tree(tree); //Uncoment this for XYZ splitting
863 }
864
865 void BLI_bvhtree_balance(BVHTree *tree)
866 {
867         if(tree->totleaf == 0) return;
868
869         assert(tree->totbranch == 0);
870         non_recursive_bvh_div_nodes(tree);
871
872 /*
873         if(tree->totleaf != 0)
874         {
875                 // create root node
876         BVHNode *node = tree->nodes[tree->totleaf] = &(tree->nodearray[tree->totleaf]);
877         tree->totbranch++;
878         <       
879                 // refit root bvh node
880         refit_kdop_hull(tree, node, 0, tree->totleaf);
881
882                 // create + balance tree
883         omp_bvh_div_nodes(tree, node, 0, tree->totleaf, tree->totleaf+1);
884         tree->totbranch = needed_branches( tree->tree_type, tree->totleaf );
885                 // verify_tree(tree);
886 }
887 */
888
889 }
890
891 // overlap - is it possbile for 2 bv's to collide ?
892 static int tree_overlap(float *bv1, float *bv2, int start_axis, int stop_axis)
893 {
894         float *bv1_end = bv1 + (stop_axis<<1);
895                 
896         bv1 += start_axis<<1;
897         bv2 += start_axis<<1;
898         
899         // test all axis if min + max overlap
900         for (; bv1 != bv1_end; bv1+=2, bv2+=2)
901         {
902                 if ((*(bv1) > *(bv2 + 1)) || (*(bv2) > *(bv1 + 1))) 
903                         return 0;
904         }
905         
906         return 1;
907 }
908
909 static void traverse(BVHOverlapData *data, BVHNode *node1, BVHNode *node2)
910 {
911         int j;
912         
913         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)))
914         {
915                 // check if node1 is a leaf
916                 if(!node1->totnode)
917                 {
918                         // check if node2 is a leaf
919                         if(!node2->totnode)
920                         {
921                                 
922                                 if(node1 == node2)
923                                 {
924                                         return;
925                                 }
926                                         
927                                 if(data->i >= data->max_overlap)
928                                 {       
929                                         // try to make alloc'ed memory bigger
930                                         data->overlap = realloc(data->overlap, sizeof(BVHTreeOverlap)*data->max_overlap*2);
931                                         
932                                         if(!data->overlap)
933                                         {
934                                                 printf("Out of Memory in traverse\n");
935                                                 return;
936                                         }
937                                         data->max_overlap *= 2;
938                                 }
939                                 
940                                 // both leafs, insert overlap!
941                                 data->overlap[data->i].indexA = node1->index;
942                                 data->overlap[data->i].indexB = node2->index;
943
944                                 data->i++;
945                         }
946                         else
947                         {
948                                 for(j = 0; j < data->tree2->tree_type; j++)
949                                 {
950                                         if(node2->children[j])
951                                                 traverse(data, node1, node2->children[j]);
952                                 }
953                         }
954                 }
955                 else
956                 {
957                         
958                         for(j = 0; j < data->tree2->tree_type; j++)
959                         {
960                                 if(node1->children[j])
961                                         traverse(data, node1->children[j], node2);
962                         }
963                 }
964         }
965         return;
966 }
967
968 BVHTreeOverlap *BLI_bvhtree_overlap(BVHTree *tree1, BVHTree *tree2, int *result)
969 {
970         int j, total = 0;
971         BVHTreeOverlap *overlap = NULL, *to = NULL;
972         BVHOverlapData **data;
973         
974         // check for compatibility of both trees (can't compare 14-DOP with 18-DOP)
975         if((tree1->axis != tree2->axis) && ((tree1->axis == 14) || tree2->axis == 14))
976                 return 0;
977         
978         // fast check root nodes for collision before doing big splitting + traversal
979         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)))
980                 return 0;
981
982         data = MEM_callocN(sizeof(BVHOverlapData *)* tree1->tree_type, "BVHOverlapData_star");
983         
984         for(j = 0; j < tree1->tree_type; j++)
985         {
986                 data[j] = (BVHOverlapData *)MEM_callocN(sizeof(BVHOverlapData), "BVHOverlapData");
987                 
988                 // init BVHOverlapData
989                 data[j]->overlap = (BVHTreeOverlap *)malloc(sizeof(BVHTreeOverlap)*MAX2(tree1->totleaf, tree2->totleaf));
990                 data[j]->tree1 = tree1;
991                 data[j]->tree2 = tree2;
992                 data[j]->max_overlap = MAX2(tree1->totleaf, tree2->totleaf);
993                 data[j]->i = 0;
994         }
995
996 #pragma omp parallel for private(j) schedule(static)
997         for(j = 0; j < MIN2(tree1->tree_type, tree1->nodes[tree1->totleaf]->totnode); j++)
998         {
999                 traverse(data[j], tree1->nodes[tree1->totleaf]->children[j], tree2->nodes[tree2->totleaf]);
1000         }
1001         
1002         for(j = 0; j < tree1->tree_type; j++)
1003                 total += data[j]->i;
1004         
1005         to = overlap = (BVHTreeOverlap *)MEM_callocN(sizeof(BVHTreeOverlap)*total, "BVHTreeOverlap");
1006         
1007         for(j = 0; j < tree1->tree_type; j++)
1008         {
1009                 memcpy(to, data[j]->overlap, data[j]->i*sizeof(BVHTreeOverlap));
1010                 to+=data[j]->i;
1011         }
1012         
1013         for(j = 0; j < tree1->tree_type; j++)
1014         {
1015                 free(data[j]->overlap);
1016                 MEM_freeN(data[j]);
1017         }
1018         MEM_freeN(data);
1019         
1020         (*result) = total;
1021         return overlap;
1022 }
1023
1024
1025
1026 // bottom up update of bvh tree:
1027 // join the 4 children here
1028 static void node_join(BVHTree *tree, BVHNode *node)
1029 {
1030         int i, j;
1031         
1032         for (i = tree->start_axis; i < tree->stop_axis; i++)
1033         {
1034                 node->bv[2*i] = FLT_MAX;
1035                 node->bv[2*i + 1] = -FLT_MAX;
1036         }
1037         
1038         for (i = 0; i < tree->tree_type; i++)
1039         {
1040                 if (node->children[i]) 
1041                 {
1042                         for (j = tree->start_axis; j < tree->stop_axis; j++)
1043                         {
1044                                 // update minimum 
1045                                 if (node->children[i]->bv[(2 * j)] < node->bv[(2 * j)]) 
1046                                         node->bv[(2 * j)] = node->children[i]->bv[(2 * j)];
1047                                 
1048                                 // update maximum 
1049                                 if (node->children[i]->bv[(2 * j) + 1] > node->bv[(2 * j) + 1])
1050                                         node->bv[(2 * j) + 1] = node->children[i]->bv[(2 * j) + 1];
1051                         }
1052                 }
1053                 else
1054                         break;
1055         }
1056 }
1057
1058 // call before BLI_bvhtree_update_tree()
1059 int BLI_bvhtree_update_node(BVHTree *tree, int index, float *co, float *co_moving, int numpoints)
1060 {
1061         BVHNode *node= NULL;
1062         int i = 0;
1063         
1064         // check if index exists
1065         if(index > tree->totleaf)
1066                 return 0;
1067         
1068         node = tree->nodearray + index;
1069         
1070         create_kdop_hull(tree, node, co, numpoints, 0);
1071         
1072         if(co_moving)
1073                 create_kdop_hull(tree, node, co_moving, numpoints, 1);
1074         
1075         // inflate the bv with some epsilon
1076         for (i = tree->start_axis; i < tree->stop_axis; i++)
1077         {
1078                 node->bv[(2 * i)] -= tree->epsilon; // minimum 
1079                 node->bv[(2 * i) + 1] += tree->epsilon; // maximum 
1080         }
1081         
1082         return 1;
1083 }
1084
1085 // call BLI_bvhtree_update_node() first for every node/point/triangle
1086 void BLI_bvhtree_update_tree(BVHTree *tree)
1087 {
1088         BVHNode *leaf, *parent;
1089         
1090         // reset tree traversing flag
1091         for (leaf = tree->nodearray + tree->totleaf; leaf != tree->nodearray + tree->totleaf + tree->totbranch; leaf++)
1092                 leaf->traversed = 0;
1093         
1094         for (leaf = tree->nodearray; leaf != tree->nodearray + tree->totleaf; leaf++)
1095         {
1096                 for (parent = leaf->parent; parent; parent = parent->parent)
1097                 {
1098                         parent->traversed++;    // we tried to go up in hierarchy 
1099                         if (parent->traversed < parent->totnode) 
1100                                 break;  // we do not need to check further 
1101                         else 
1102                                 node_join(tree, parent);
1103                 }
1104         }
1105 }
1106
1107 float BLI_bvhtree_getepsilon(BVHTree *tree)
1108 {
1109         return tree->epsilon;
1110 }
1111
1112
1113
1114
1115 /*
1116  * Nearest neighbour
1117  */
1118 static float squared_dist(const float *a, const float *b)
1119 {
1120         float tmp[3];
1121         VECSUB(tmp, a, b);
1122         return INPR(tmp, tmp);
1123 }
1124
1125 static float calc_nearest_point(BVHNearestData *data, BVHNode *node, float *nearest)
1126 {
1127         int i;
1128         const float *bv = node->bv;
1129
1130         //nearest on AABB hull
1131         for(i=0; i != 3; i++, bv += 2)
1132         {
1133                 if(bv[0] > data->proj[i])
1134                         nearest[i] = bv[0];
1135                 else if(bv[1] < data->proj[i])
1136                         nearest[i] = bv[1];
1137                 else
1138                         nearest[i] = data->proj[i];
1139         }
1140
1141 /*
1142         //nearest on a general hull
1143         VECCOPY(nearest, data->co);
1144         for(i = data->tree->start_axis; i != data->tree->stop_axis; i++, bv+=2)
1145         {
1146         float proj = INPR( nearest, KDOP_AXES[i]);
1147         float dl = bv[0] - proj;
1148         float du = bv[1] - proj;
1149
1150         if(dl > 0)
1151         {
1152         VECADDFAC(nearest, nearest, KDOP_AXES[i], dl);
1153 }
1154         else if(du < 0)
1155         {
1156         VECADDFAC(nearest, nearest, KDOP_AXES[i], du);
1157 }
1158 }
1159 */
1160         return squared_dist(data->co, nearest);
1161 }
1162
1163
1164 // TODO: use a priority queue to reduce the number of nodes looked on
1165 static void dfs_find_nearest(BVHNearestData *data, BVHNode *node)
1166 {
1167         int i;
1168         float nearest[3], sdist;
1169
1170         sdist = calc_nearest_point(data, node, nearest);
1171         if(sdist >= data->nearest.dist) return;
1172
1173         if(node->totnode == 0)
1174         {
1175                 if(data->callback)
1176                         data->callback(data->userdata , node->index, data->co, &data->nearest);
1177                 else
1178                 {
1179                         data->nearest.index     = node->index;
1180                         VECCOPY(data->nearest.co, nearest);
1181                         data->nearest.dist      = sdist;
1182                 }
1183         }
1184         else
1185         {
1186                 for(i=0; i != node->totnode; i++)
1187                         dfs_find_nearest(data, node->children[i]);
1188         }
1189 }
1190
1191 int BLI_bvhtree_find_nearest(BVHTree *tree, const float *co, BVHTreeNearest *nearest, BVHTree_NearestPointCallback callback, void *userdata)
1192 {
1193         int i;
1194
1195         BVHNearestData data;
1196
1197         //init data to search
1198         data.tree = tree;
1199         data.co = co;
1200
1201         data.callback = callback;
1202         data.userdata = userdata;
1203
1204         for(i = data.tree->start_axis; i != data.tree->stop_axis; i++)
1205         {
1206                 data.proj[i] = INPR(data.co, KDOP_AXES[i]);
1207         }
1208
1209         if(nearest)
1210         {
1211                 memcpy( &data.nearest , nearest, sizeof(*nearest) );
1212         }
1213         else
1214         {
1215                 data.nearest.index = -1;
1216                 data.nearest.dist = FLT_MAX;
1217         }
1218
1219         //dfs search
1220         dfs_find_nearest(&data, tree->nodes[tree->totleaf] );
1221
1222         //copy back results
1223         if(nearest)
1224         {
1225                 memcpy(nearest, &data.nearest, sizeof(*nearest));
1226         }
1227
1228         return data.nearest.index;
1229 }
1230
1231
1232
1233 /*
1234  * Ray cast
1235  */
1236
1237 static float ray_nearest_hit(BVHRayCastData *data, BVHNode *node)
1238 {
1239         int i;
1240         const float *bv = node->bv;
1241
1242         float low = 0, upper = data->hit.dist;
1243
1244         for(i=0; i != 3; i++, bv += 2)
1245         {
1246                 if(data->ray_dot_axis[i] == 0.0f)
1247                 {
1248                         //axis aligned ray
1249                         if(data->ray.origin[i] < bv[0]
1250                                                 || data->ray.origin[i] > bv[1])
1251                                 return FLT_MAX;
1252                 }
1253                 else
1254                 {
1255                         float ll = (bv[0] - data->ray.origin[i]) / data->ray_dot_axis[i];
1256                         float lu = (bv[1] - data->ray.origin[i]) / data->ray_dot_axis[i];
1257
1258                         if(data->ray_dot_axis[i] > 0)
1259                         {
1260                                 if(ll > low)   low = ll;
1261                                 if(lu < upper) upper = lu;
1262                         }
1263                         else
1264                         {
1265                                 if(lu > low)   low = lu;
1266                                 if(ll < upper) upper = ll;
1267                         }
1268         
1269                         if(low > upper) return FLT_MAX;
1270                 }
1271         }
1272         return low;
1273 }
1274
1275 static void dfs_raycast(BVHRayCastData *data, BVHNode *node)
1276 {
1277         int i;
1278
1279         //ray-bv is really fast.. and simple tests revealed its worth to test it
1280         //before calling the ray-primitive functions
1281         float dist = ray_nearest_hit(data, node);
1282         if(dist >= data->hit.dist) return;
1283
1284         if(node->totnode == 0)
1285         {
1286                 if(data->callback)
1287                         data->callback(data->userdata, node->index, &data->ray, &data->hit);
1288                 else
1289                 {
1290                         data->hit.index = node->index;
1291                         data->hit.dist  = dist;
1292                         VECADDFAC(data->hit.co, data->ray.origin, data->ray.direction, dist);
1293                 }
1294         }
1295         else
1296         {
1297                 //pick loop direction to dive into the tree (based on ray direction and split axis)
1298                 if(data->ray_dot_axis[ node->main_axis ] > 0)
1299                 {
1300                         for(i=0; i != node->totnode; i++)
1301                         {
1302                                 dfs_raycast(data, node->children[i]);
1303                         }
1304                 }
1305                 else
1306                 {
1307                         for(i=node->totnode-1; i >= 0; i--)
1308                         {
1309                                 dfs_raycast(data, node->children[i]);
1310                         }
1311                 }
1312         }
1313 }
1314
1315
1316
1317 int BLI_bvhtree_ray_cast(BVHTree *tree, const float *co, const float *dir, BVHTreeRayHit *hit, BVHTree_RayCastCallback callback, void *userdata)
1318 {
1319         int i;
1320         BVHRayCastData data;
1321
1322         data.tree = tree;
1323
1324         data.callback = callback;
1325         data.userdata = userdata;
1326
1327         VECCOPY(data.ray.origin,    co);
1328         VECCOPY(data.ray.direction, dir);
1329
1330         Normalize(data.ray.direction);
1331
1332         for(i=0; i<3; i++)
1333         {
1334                 data.ray_dot_axis[i] = INPR( data.ray.direction, KDOP_AXES[i]);
1335
1336                 if(fabs(data.ray_dot_axis[i]) < 1e-7)
1337                         data.ray_dot_axis[i] = 0.0;
1338         }
1339
1340
1341         if(hit)
1342                 memcpy( &data.hit, hit, sizeof(*hit) );
1343         else
1344         {
1345                 data.hit.index = -1;
1346                 data.hit.dist = FLT_MAX;
1347         }
1348
1349         dfs_raycast(&data, tree->nodes[tree->totleaf]);
1350
1351
1352         if(hit)
1353                 memcpy( hit, &data.hit, sizeof(*hit) );
1354
1355         return data.hit.index;
1356 }
1357