Merge with -r 22620:23107.
[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 #include <assert.h>
34
35 #include "MEM_guardedalloc.h"
36
37 #include "BKE_utildefines.h"
38
39 #include "BLI_kdopbvh.h"
40 #include "BLI_arithb.h"
41
42 #ifdef _OPENMP
43 #include <omp.h>
44 #endif
45
46
47
48 #define MAX_TREETYPE 32
49 #define DEFAULT_FIND_NEAREST_HEAP_SIZE 1024
50
51 typedef struct BVHNode
52 {
53         struct BVHNode **children;
54         struct BVHNode *parent; // some user defined traversed need that
55         float *bv;              // Bounding volume of all nodes, max 13 axis
56         int index;              // face, edge, vertex index
57         char totnode;   // how many nodes are used, used for speedup
58         char main_axis; // Axis used to split this node
59 } BVHNode;
60
61 struct BVHTree
62 {
63         BVHNode **nodes;
64         BVHNode *nodearray; /* pre-alloc branch nodes */
65         BVHNode **nodechild;    // pre-alloc childs for nodes
66         float   *nodebv;                // pre-alloc bounding-volumes for nodes
67         float   epsilon; /* epslion is used for inflation of the k-dop     */
68         int     totleaf; // leafs
69         int     totbranch;
70         char    tree_type; // type of tree (4 => quadtree)
71         char    axis; // kdop type (6 => OBB, 7 => AABB, ...)
72         char    start_axis, stop_axis; // KDOP_AXES array indices according to axis
73 };
74
75 typedef struct BVHOverlapData 
76 {  
77         BVHTree *tree1, *tree2; 
78         BVHTreeOverlap *overlap; 
79         int i, max_overlap; /* i is number of overlaps */
80         int start_axis, stop_axis;
81 } BVHOverlapData;
82
83 typedef struct BVHNearestData
84 {
85         BVHTree *tree;
86         const float     *co;
87         BVHTree_NearestPointCallback callback;
88         void    *userdata;
89         float proj[13];                 //coordinates projection over axis
90         BVHTreeNearest nearest;
91
92 } BVHNearestData;
93
94 typedef struct BVHRayCastData
95 {
96         BVHTree *tree;
97
98         BVHTree_RayCastCallback callback;
99         void    *userdata;
100
101
102         BVHTreeRay    ray;
103         float ray_dot_axis[13];
104
105         BVHTreeRayHit hit;
106 } BVHRayCastData;
107 ////////////////////////////////////////m
108
109
110 ////////////////////////////////////////////////////////////////////////
111 // Bounding Volume Hierarchy Definition
112 // 
113 // Notes: From OBB until 26-DOP --> all bounding volumes possible, just choose type below
114 // Notes: You have to choose the type at compile time ITM
115 // Notes: You can choose the tree type --> binary, quad, octree, choose below
116 ////////////////////////////////////////////////////////////////////////
117
118 static float KDOP_AXES[13][3] =
119 { {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},
120 {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},
121 {0, 1.0, -1.0}
122 };
123
124 /*
125  * Generic push and pop heap
126  */
127 #define PUSH_HEAP_BODY(HEAP_TYPE,PRIORITY,heap,heap_size)       \
128 {                                                                                                       \
129         HEAP_TYPE element = heap[heap_size-1];                  \
130         int child = heap_size-1;                                                \
131         while(child != 0)                                                               \
132         {                                                                                               \
133                 int parent = (child-1) / 2;                                     \
134                 if(PRIORITY(element, heap[parent]))                     \
135                 {                                                                                       \
136                         heap[child] = heap[parent];                             \
137                         child = parent;                                                 \
138                 }                                                                                       \
139                 else break;                                                                     \
140         }                                                                                               \
141         heap[child] = element;                                                  \
142 }
143
144 #define POP_HEAP_BODY(HEAP_TYPE, PRIORITY,heap,heap_size)       \
145 {                                                                                                       \
146         HEAP_TYPE element = heap[heap_size-1];                  \
147         int parent = 0;                                                                 \
148         while(parent < (heap_size-1)/2 )                                \
149         {                                                                                               \
150                 int child2 = (parent+1)*2;                                      \
151                 if(PRIORITY(heap[child2-1], heap[child2]))      \
152                         --child2;                                                               \
153                                                                                                         \
154                 if(PRIORITY(element, heap[child2]))                     \
155                         break;                                                                  \
156                                                                                                         \
157                 heap[parent] = heap[child2];                            \
158                 parent = child2;                                                        \
159         }                                                                                               \
160         heap[parent] = element;                                                 \
161 }
162
163 int ADJUST_MEMORY(void *local_memblock, void **memblock, int new_size, int *max_size, int size_per_item)
164 {
165         int   new_max_size = *max_size * 2;
166         void *new_memblock = NULL;
167
168         if(new_size <= *max_size)
169                 return TRUE;
170
171         if(*memblock == local_memblock)
172         {
173                 new_memblock = malloc( size_per_item * new_max_size );
174                 memcpy( new_memblock, *memblock, size_per_item * *max_size );
175         }
176         else
177                 new_memblock = realloc(*memblock, size_per_item * new_max_size );
178
179         if(new_memblock)
180         {
181                 *memblock = new_memblock;
182                 *max_size = new_max_size;
183                 return TRUE;
184         }
185         else
186                 return FALSE;
187 }
188
189
190 //////////////////////////////////////////////////////////////////////////////////////////////////////
191 // Introsort 
192 // with permission deriven from the following Java code:
193 // http://ralphunden.net/content/tutorials/a-guide-to-introsort/
194 // and he derived it from the SUN STL 
195 //////////////////////////////////////////////////////////////////////////////////////////////////////
196 static int size_threshold = 16;
197 /*
198 * Common methods for all algorithms
199 */
200 static int floor_lg(int a)
201 {
202         return (int)(floor(log(a)/log(2)));
203 }
204
205 /*
206 * Insertion sort algorithm
207 */
208 static void bvh_insertionsort(BVHNode **a, int lo, int hi, int axis)
209 {
210         int i,j;
211         BVHNode *t;
212         for (i=lo; i < hi; i++)
213         {
214                 j=i;
215                 t = a[i];
216                 while((j!=lo) && (t->bv[axis] < (a[j-1])->bv[axis]))
217                 {
218                         a[j] = a[j-1];
219                         j--;
220                 }
221                 a[j] = t;
222         }
223 }
224
225 static int bvh_partition(BVHNode **a, int lo, int hi, BVHNode * x, int axis)
226 {
227         int i=lo, j=hi;
228         while (1)
229         {
230                 while ((a[i])->bv[axis] < x->bv[axis]) i++;
231                 j--;
232                 while (x->bv[axis] < (a[j])->bv[axis]) j--;
233                 if(!(i < j))
234                         return i;
235                 SWAP( BVHNode* , a[i], a[j]);
236                 i++;
237         }
238 }
239
240 /*
241 * Heapsort algorithm
242 */
243 static void bvh_downheap(BVHNode **a, int i, int n, int lo, int axis)
244 {
245         BVHNode * d = a[lo+i-1];
246         int child;
247         while (i<=n/2)
248         {
249                 child = 2*i;
250                 if ((child < n) && ((a[lo+child-1])->bv[axis] < (a[lo+child])->bv[axis]))
251                 {
252                         child++;
253                 }
254                 if (!(d->bv[axis] < (a[lo+child-1])->bv[axis])) break;
255                 a[lo+i-1] = a[lo+child-1];
256                 i = child;
257         }
258         a[lo+i-1] = d;
259 }
260
261 static void bvh_heapsort(BVHNode **a, int lo, int hi, int axis)
262 {
263         int n = hi-lo, i;
264         for (i=n/2; i>=1; i=i-1)
265         {
266                 bvh_downheap(a, i,n,lo, axis);
267         }
268         for (i=n; i>1; i=i-1)
269         {
270                 SWAP(BVHNode*, a[lo],a[lo+i-1]);
271                 bvh_downheap(a, 1,i-1,lo, axis);
272         }
273 }
274
275 static BVHNode *bvh_medianof3(BVHNode **a, int lo, int mid, int hi, int axis) // returns Sortable
276 {
277         if ((a[mid])->bv[axis] < (a[lo])->bv[axis])
278         {
279                 if ((a[hi])->bv[axis] < (a[mid])->bv[axis])
280                         return a[mid];
281                 else
282                 {
283                         if ((a[hi])->bv[axis] < (a[lo])->bv[axis])
284                                 return a[hi];
285                         else
286                                 return a[lo];
287                 }
288         }
289         else
290         {
291                 if ((a[hi])->bv[axis] < (a[mid])->bv[axis])
292                 {
293                         if ((a[hi])->bv[axis] < (a[lo])->bv[axis])
294                                 return a[lo];
295                         else
296                                 return a[hi];
297                 }
298                 else
299                         return a[mid];
300         }
301 }
302 /*
303 * Quicksort algorithm modified for Introsort
304 */
305 static void bvh_introsort_loop (BVHNode **a, int lo, int hi, int depth_limit, int axis)
306 {
307         int p;
308
309         while (hi-lo > size_threshold)
310         {
311                 if (depth_limit == 0)
312                 {
313                         bvh_heapsort(a, lo, hi, axis);
314                         return;
315                 }
316                 depth_limit=depth_limit-1;
317                 p=bvh_partition(a, lo, hi, bvh_medianof3(a, lo, lo+((hi-lo)/2)+1, hi-1, axis), axis);
318                 bvh_introsort_loop(a, p, hi, depth_limit, axis);
319                 hi=p;
320         }
321 }
322
323 static void sort(BVHNode **a0, int begin, int end, int axis)
324 {
325         if (begin < end)
326         {
327                 BVHNode **a=a0;
328                 bvh_introsort_loop(a, begin, end, 2*floor_lg(end-begin), axis);
329                 bvh_insertionsort(a, begin, end, axis);
330         }
331 }
332 void sort_along_axis(BVHTree *tree, int start, int end, int axis)
333 {
334         sort(tree->nodes, start, end, axis);
335 }
336
337 //after a call to this function you can expect one of:
338 //      every node to left of a[n] are smaller or equal to it
339 //      every node to the right of a[n] are greater or equal to it
340 int partition_nth_element(BVHNode **a, int _begin, int _end, int n, int axis){
341         int begin = _begin, end = _end, cut;
342         while(end-begin > 3)
343         {
344                 cut = bvh_partition(a, begin, end, bvh_medianof3(a, begin, (begin+end)/2, end-1, axis), axis );
345                 if(cut <= n)
346                         begin = cut;
347                 else
348                         end = cut;
349         }
350         bvh_insertionsort(a, begin, end, axis);
351
352         return n;
353 }
354
355 //////////////////////////////////////////////////////////////////////////////////////////////////////
356
357 /*
358  * BVHTree bounding volumes functions
359  */
360 static void create_kdop_hull(BVHTree *tree, BVHNode *node, float *co, int numpoints, int moving)
361 {
362         float newminmax;
363         float *bv = node->bv;
364         int i, k;
365         
366         // don't init boudings for the moving case
367         if(!moving)
368         {
369                 for (i = tree->start_axis; i < tree->stop_axis; i++)
370                 {
371                         bv[2*i] = FLT_MAX;
372                         bv[2*i + 1] = -FLT_MAX;
373                 }
374         }
375         
376         for(k = 0; k < numpoints; k++)
377         {
378                 // for all Axes.
379                 for (i = tree->start_axis; i < tree->stop_axis; i++)
380                 {
381                         newminmax = INPR(&co[k * 3], KDOP_AXES[i]);
382                         if (newminmax < bv[2 * i])
383                                 bv[2 * i] = newminmax;
384                         if (newminmax > bv[(2 * i) + 1])
385                                 bv[(2 * i) + 1] = newminmax;
386                 }
387         }
388 }
389
390 // depends on the fact that the BVH's for each face is already build
391 static void refit_kdop_hull(BVHTree *tree, BVHNode *node, int start, int end)
392 {
393         float newmin,newmax;
394         int i, j;
395         float *bv = node->bv;
396
397         
398         for (i = tree->start_axis; i < tree->stop_axis; i++)
399         {
400                 bv[2*i] = FLT_MAX;
401                 bv[2*i + 1] = -FLT_MAX;
402         }
403
404         for (j = start; j < end; j++)
405         {
406 // for all Axes.
407                 for (i = tree->start_axis; i < tree->stop_axis; i++)
408                 {
409                         newmin = tree->nodes[j]->bv[(2 * i)];   
410                         if ((newmin < bv[(2 * i)]))
411                                 bv[(2 * i)] = newmin;
412  
413                         newmax = tree->nodes[j]->bv[(2 * i) + 1];
414                         if ((newmax > bv[(2 * i) + 1]))
415                                 bv[(2 * i) + 1] = newmax;
416                 }
417         }
418
419 }
420
421 // only supports x,y,z axis in the moment
422 // but we should use a plain and simple function here for speed sake
423 static char get_largest_axis(float *bv)
424 {
425         float middle_point[3];
426
427         middle_point[0] = (bv[1]) - (bv[0]); // x axis
428         middle_point[1] = (bv[3]) - (bv[2]); // y axis
429         middle_point[2] = (bv[5]) - (bv[4]); // z axis
430         if (middle_point[0] > middle_point[1]) 
431         {
432                 if (middle_point[0] > middle_point[2])
433                         return 1; // max x axis
434                 else
435                         return 5; // max z axis
436         }
437         else 
438         {
439                 if (middle_point[1] > middle_point[2])
440                         return 3; // max y axis
441                 else
442                         return 5; // max z axis
443         }
444 }
445
446 // bottom-up update of bvh node BV
447 // join the children on the parent BV
448 static void node_join(BVHTree *tree, BVHNode *node)
449 {
450         int i, j;
451         
452         for (i = tree->start_axis; i < tree->stop_axis; i++)
453         {
454                 node->bv[2*i] = FLT_MAX;
455                 node->bv[2*i + 1] = -FLT_MAX;
456         }
457         
458         for (i = 0; i < tree->tree_type; i++)
459         {
460                 if (node->children[i]) 
461                 {
462                         for (j = tree->start_axis; j < tree->stop_axis; j++)
463                         {
464                                 // update minimum 
465                                 if (node->children[i]->bv[(2 * j)] < node->bv[(2 * j)]) 
466                                         node->bv[(2 * j)] = node->children[i]->bv[(2 * j)];
467                                 
468                                 // update maximum 
469                                 if (node->children[i]->bv[(2 * j) + 1] > node->bv[(2 * j) + 1])
470                                         node->bv[(2 * j) + 1] = node->children[i]->bv[(2 * j) + 1];
471                         }
472                 }
473                 else
474                         break;
475         }
476 }
477
478 /*
479  * Debug and information functions
480  */
481 #if 0
482 static void bvhtree_print_tree(BVHTree *tree, BVHNode *node, int depth)
483 {
484         int i;
485         for(i=0; i<depth; i++) printf(" ");
486         printf(" - %d (%ld): ", node->index, node - tree->nodearray);
487         for(i=2*tree->start_axis; i<2*tree->stop_axis; i++)
488                 printf("%.3f ", node->bv[i]);
489         printf("\n");
490
491         for(i=0; i<tree->tree_type; i++)
492                 if(node->children[i])
493                         bvhtree_print_tree(tree, node->children[i], depth+1);
494 }
495
496 static void bvhtree_info(BVHTree *tree)
497 {
498         printf("BVHTree info\n");
499         printf("tree_type = %d, axis = %d, epsilon = %f\n", tree->tree_type, tree->axis, tree->epsilon);
500         printf("nodes = %d, branches = %d, leafs = %d\n", tree->totbranch + tree->totleaf,  tree->totbranch, tree->totleaf);
501         printf("Memory per node = %ldbytes\n", sizeof(BVHNode) + sizeof(BVHNode*)*tree->tree_type + sizeof(float)*tree->axis);
502         printf("BV memory = %dbytes\n", MEM_allocN_len(tree->nodebv));
503
504         printf("Total memory = %ldbytes\n", sizeof(BVHTree)
505                 + MEM_allocN_len(tree->nodes)
506                 + MEM_allocN_len(tree->nodearray)
507                 + MEM_allocN_len(tree->nodechild)
508                 + MEM_allocN_len(tree->nodebv)
509                 );
510
511 //      bvhtree_print_tree(tree, tree->nodes[tree->totleaf], 0);
512 }
513 #endif
514
515 #if 0
516
517
518 static void verify_tree(BVHTree *tree)
519 {
520         int i, j, check = 0;
521         
522         // check the pointer list
523         for(i = 0; i < tree->totleaf; i++)
524         {
525                 if(tree->nodes[i]->parent == NULL)
526                         printf("Leaf has no parent: %d\n", i);
527                 else
528                 {
529                         for(j = 0; j < tree->tree_type; j++)
530                         {
531                                 if(tree->nodes[i]->parent->children[j] == tree->nodes[i])
532                                         check = 1;
533                         }
534                         if(!check)
535                         {
536                                 printf("Parent child relationship doesn't match: %d\n", i);
537                         }
538                         check = 0;
539                 }
540         }
541         
542         // check the leaf list
543         for(i = 0; i < tree->totleaf; i++)
544         {
545                 if(tree->nodearray[i].parent == NULL)
546                         printf("Leaf has no parent: %d\n", i);
547                 else
548                 {
549                         for(j = 0; j < tree->tree_type; j++)
550                         {
551                                 if(tree->nodearray[i].parent->children[j] == &tree->nodearray[i])
552                                         check = 1;
553                         }
554                         if(!check)
555                         {
556                                 printf("Parent child relationship doesn't match: %d\n", i);
557                         }
558                         check = 0;
559                 }
560         }
561         
562         printf("branches: %d, leafs: %d, total: %d\n", tree->totbranch, tree->totleaf, tree->totbranch + tree->totleaf);
563 }
564 #endif
565
566 //Helper data and structures to build a min-leaf generalized implicit tree
567 //This code can be easily reduced (basicly this is only method to calculate pow(k, n) in O(1).. and stuff like that)
568 typedef struct BVHBuildHelper
569 {
570         int tree_type;                          //
571         int totleafs;                           //
572
573         int leafs_per_child  [32];      //Min number of leafs that are archievable from a node at depth N
574         int branches_on_level[32];      //Number of nodes at depth N (tree_type^N)
575
576         int remain_leafs;                       //Number of leafs that are placed on the level that is not 100% filled
577
578 } BVHBuildHelper;
579
580 static void build_implicit_tree_helper(BVHTree *tree, BVHBuildHelper *data)
581 {
582         int depth = 0;
583         int remain;
584         int nnodes;
585
586         data->totleafs = tree->totleaf;
587         data->tree_type= tree->tree_type;
588
589         //Calculate the smallest tree_type^n such that tree_type^n >= num_leafs
590         for(
591                 data->leafs_per_child[0] = 1;
592                 data->leafs_per_child[0] <  data->totleafs;
593                 data->leafs_per_child[0] *= data->tree_type
594         );
595
596         data->branches_on_level[0] = 1;
597
598         //We could stop the loop first (but I am lazy to find out when)
599         for(depth = 1; depth < 32; depth++)
600         {
601                 data->branches_on_level[depth] = data->branches_on_level[depth-1] * data->tree_type;
602                 data->leafs_per_child  [depth] = data->leafs_per_child  [depth-1] / data->tree_type;
603         }
604
605         remain = data->totleafs - data->leafs_per_child[1];
606         nnodes = (remain + data->tree_type - 2) / (data->tree_type - 1);
607         data->remain_leafs = remain + nnodes;
608 }
609
610 // return the min index of all the leafs archivable with the given branch
611 static int implicit_leafs_index(BVHBuildHelper *data, int depth, int child_index)
612 {
613         int min_leaf_index = child_index * data->leafs_per_child[depth-1];
614         if(min_leaf_index <= data->remain_leafs)
615                 return min_leaf_index;
616         else if(data->leafs_per_child[depth])
617                 return data->totleafs - (data->branches_on_level[depth-1] - child_index) * data->leafs_per_child[depth];
618         else
619                 return data->remain_leafs;
620 }
621
622 /**
623  * Generalized implicit tree build
624  *
625  * An implicit tree is a tree where its structure is implied, thus there is no need to store child pointers or indexs.
626  * Its possible to find the position of the child or the parent with simple maths (multiplication and adittion). This type
627  * of tree is for example used on heaps.. where node N has its childs at indexs N*2 and N*2+1.
628  *
629  * Altought in this case the tree type is general.. and not know until runtime.
630  * tree_type stands for the maximum number of childs that a tree node can have.
631  * All tree types >= 2 are supported.
632  *
633  * Advantages of the used trees include:
634  *  - No need to store child/parent relations (they are implicit);
635  *  - Any node child always has an index greater than the parent;
636  *  - Brother nodes are sequencial in memory;
637  *
638  *
639  * Some math relations derived for general implicit trees:
640  *
641  *   K = tree_type, ( 2 <= K )
642  *   ROOT = 1
643  *   N child of node A = A * K + (2 - K) + N, (0 <= N < K)
644  *
645  * Util methods:
646  *   TODO...
647  *    (looping elements, knowing if its a leaf or not.. etc...)
648  */
649
650 // This functions returns the number of branches needed to have the requested number of leafs.
651 static int implicit_needed_branches(int tree_type, int leafs)
652 {
653         return MAX2(1, (leafs + tree_type - 3) / (tree_type-1) );
654 }
655
656 /*
657  * This function handles the problem of "sorting" the leafs (along the split_axis).
658  *
659  * It arranges the elements in the given partitions such that:
660  *  - any element in partition N is less or equal to any element in partition N+1.
661  *  - if all elements are diferent all partition will get the same subset of elements
662  *    as if the array was sorted.
663  *
664  * partition P is described as the elements in the range ( nth[P] , nth[P+1] ]
665  *
666  * TODO: This can be optimized a bit by doing a specialized nth_element instead of K nth_elements
667  */
668 static void split_leafs(BVHNode **leafs_array, int *nth, int partitions, int split_axis)
669 {
670         int i;
671         for(i=0; i < partitions-1; i++)
672         {
673                 if(nth[i] >= nth[partitions])
674                         break;
675
676                 partition_nth_element(leafs_array, nth[i], nth[partitions], nth[i+1], split_axis);
677         }
678 }
679
680 /*
681  * This functions builds an optimal implicit tree from the given leafs.
682  * Where optimal stands for:
683  *  - The resulting tree will have the smallest number of branches;
684  *  - At most only one branch will have NULL childs;
685  *  - All leafs will be stored at level N or N+1.
686  *
687  * This function creates an implicit tree on branches_array, the leafs are given on the leafs_array.
688  *
689  * The tree is built per depth levels. First branchs at depth 1.. then branches at depth 2.. etc..
690  * The reason is that we can build level N+1 from level N witouth any data dependencies.. thus it allows
691  * to use multithread building.
692  *
693  * To archieve this is necessary to find how much leafs are accessible from a certain branch, BVHBuildHelper
694  * implicit_needed_branches and implicit_leafs_index are auxiliar functions to solve that "optimal-split".
695  */
696 static void non_recursive_bvh_div_nodes(BVHTree *tree, BVHNode *branches_array, BVHNode **leafs_array, int num_leafs)
697 {
698         int i;
699
700         const int tree_type   = tree->tree_type;
701         const int tree_offset = 2 - tree->tree_type; //this value is 0 (on binary trees) and negative on the others
702         const int num_branches= implicit_needed_branches(tree_type, num_leafs);
703
704         BVHBuildHelper data;
705         int depth;
706         
707         // set parent from root node to NULL
708         BVHNode *tmp = branches_array+0;
709         tmp->parent = NULL;
710
711         //Most of bvhtree code relies on 1-leaf trees having at least one branch
712         //We handle that special case here
713         if(num_leafs == 1)
714         {
715                 BVHNode *root = branches_array+0;
716                 refit_kdop_hull(tree, root, 0, num_leafs);
717                 root->main_axis = get_largest_axis(root->bv) / 2;
718                 root->totnode = 1;
719                 root->children[0] = leafs_array[0];
720                 root->children[0]->parent = root;
721                 return;
722         }
723
724         branches_array--;       //Implicit trees use 1-based indexs
725         
726         build_implicit_tree_helper(tree, &data);
727
728         //Loop tree levels (log N) loops
729         for(i=1, depth = 1; i <= num_branches; i = i*tree_type + tree_offset, depth++)
730         {
731                 const int first_of_next_level = i*tree_type + tree_offset;
732                 const int  end_j = MIN2(first_of_next_level, num_branches + 1); //index of last branch on this level
733                 int j;
734
735                 //Loop all branches on this level
736 #pragma omp parallel for private(j) schedule(static)
737                 for(j = i; j < end_j; j++)
738                 {
739                         int k;
740                         const int parent_level_index= j-i;
741                         BVHNode* parent = branches_array + j;
742                         int nth_positions[ MAX_TREETYPE + 1];
743                         char split_axis;
744
745                         int parent_leafs_begin = implicit_leafs_index(&data, depth, parent_level_index);
746                         int parent_leafs_end   = implicit_leafs_index(&data, depth, parent_level_index+1);
747
748                         //This calculates the bounding box of this branch
749                         //and chooses the largest axis as the axis to divide leafs
750                         refit_kdop_hull(tree, parent, parent_leafs_begin, parent_leafs_end);
751                         split_axis = get_largest_axis(parent->bv);
752
753                         //Save split axis (this can be used on raytracing to speedup the query time)
754                         parent->main_axis = split_axis / 2;
755
756                         //Split the childs along the split_axis, note: its not needed to sort the whole leafs array
757                         //Only to assure that the elements are partioned on a way that each child takes the elements
758                         //it would take in case the whole array was sorted.
759                         //Split_leafs takes care of that "sort" problem.
760                         nth_positions[        0] = parent_leafs_begin;
761                         nth_positions[tree_type] = parent_leafs_end;
762                         for(k = 1; k < tree_type; k++)
763                         {
764                                 int child_index = j * tree_type + tree_offset + k;
765                                 int child_level_index = child_index - first_of_next_level; //child level index
766                                 nth_positions[k] = implicit_leafs_index(&data, depth+1, child_level_index);
767                         }
768
769                         split_leafs(leafs_array, nth_positions, tree_type, split_axis);
770
771
772                         //Setup children and totnode counters
773                         //Not really needed but currently most of BVH code relies on having an explicit children structure
774                         for(k = 0; k < tree_type; k++)
775                         {
776                                 int child_index = j * tree_type + tree_offset + k;
777                                 int child_level_index = child_index - first_of_next_level; //child level index
778
779                                 int child_leafs_begin = implicit_leafs_index(&data, depth+1, child_level_index);
780                                 int child_leafs_end   = implicit_leafs_index(&data, depth+1, child_level_index+1);
781
782                                 if(child_leafs_end - child_leafs_begin > 1)
783                                 {
784                                         parent->children[k] = branches_array + child_index;
785                                         parent->children[k]->parent = parent;
786                                 }
787                                 else if(child_leafs_end - child_leafs_begin == 1)
788                                 {
789                                         parent->children[k] = leafs_array[ child_leafs_begin ];
790                                         parent->children[k]->parent = parent;
791                                 }
792                                 else
793                                         break;
794
795                                 parent->totnode = k+1;
796                         }
797                 }
798         }
799 }
800
801
802 /*
803  * BLI_bvhtree api
804  */
805 BVHTree *BLI_bvhtree_new(int maxsize, float epsilon, char tree_type, char axis)
806 {
807         BVHTree *tree;
808         int numnodes, i;
809         
810         // theres not support for trees below binary-trees :P
811         if(tree_type < 2)
812                 return NULL;
813         
814         if(tree_type > MAX_TREETYPE)
815                 return NULL;
816
817         tree = (BVHTree *)MEM_callocN(sizeof(BVHTree), "BVHTree");
818
819         //tree epsilon must be >= FLT_EPSILON
820         //so that tangent rays can still hit a bounding volume..
821         //this bug would show up when casting a ray aligned with a kdop-axis and with an edge of 2 faces
822         epsilon = MAX2(FLT_EPSILON, epsilon);
823         
824         if(tree)
825         {
826                 tree->epsilon = epsilon;
827                 tree->tree_type = tree_type; 
828                 tree->axis = axis;
829                 
830                 if(axis == 26)
831                 {
832                         tree->start_axis = 0;
833                         tree->stop_axis = 13;
834                 }
835                 else if(axis == 18)
836                 {
837                         tree->start_axis = 7;
838                         tree->stop_axis = 13;
839                 }
840                 else if(axis == 14)
841                 {
842                         tree->start_axis = 0;
843                         tree->stop_axis = 7;
844                 }
845                 else if(axis == 8) // AABB
846                 {
847                         tree->start_axis = 0;
848                         tree->stop_axis = 4;
849                 }
850                 else if(axis == 6) // OBB
851                 {
852                         tree->start_axis = 0;
853                         tree->stop_axis = 3;
854                 }
855                 else
856                 {
857                         MEM_freeN(tree);
858                         return NULL;
859                 }
860
861
862                 //Allocate arrays
863                 numnodes = maxsize + implicit_needed_branches(tree_type, maxsize) + tree_type;
864
865                 tree->nodes = (BVHNode **)MEM_callocN(sizeof(BVHNode *)*numnodes, "BVHNodes");
866                 
867                 if(!tree->nodes)
868                 {
869                         MEM_freeN(tree);
870                         return NULL;
871                 }
872                 
873                 tree->nodebv = (float*)MEM_callocN(sizeof(float)* axis * numnodes, "BVHNodeBV");
874                 if(!tree->nodebv)
875                 {
876                         MEM_freeN(tree->nodes);
877                         MEM_freeN(tree);
878                 }
879
880                 tree->nodechild = (BVHNode**)MEM_callocN(sizeof(BVHNode*) * tree_type * numnodes, "BVHNodeBV");
881                 if(!tree->nodechild)
882                 {
883                         MEM_freeN(tree->nodebv);
884                         MEM_freeN(tree->nodes);
885                         MEM_freeN(tree);
886                 }
887
888                 tree->nodearray = (BVHNode *)MEM_callocN(sizeof(BVHNode)* numnodes, "BVHNodeArray");
889                 
890                 if(!tree->nodearray)
891                 {
892                         MEM_freeN(tree->nodechild);
893                         MEM_freeN(tree->nodebv);
894                         MEM_freeN(tree->nodes);
895                         MEM_freeN(tree);
896                         return NULL;
897                 }
898
899                 //link the dynamic bv and child links
900                 for(i=0; i< numnodes; i++)
901                 {
902                         tree->nodearray[i].bv = tree->nodebv + i * axis;
903                         tree->nodearray[i].children = tree->nodechild + i * tree_type;
904                 }
905                 
906         }
907
908         return tree;
909 }
910
911 void BLI_bvhtree_free(BVHTree *tree)
912 {       
913         if(tree)
914         {
915                 MEM_freeN(tree->nodes);
916                 MEM_freeN(tree->nodearray);
917                 MEM_freeN(tree->nodebv);
918                 MEM_freeN(tree->nodechild);
919                 MEM_freeN(tree);
920         }
921 }
922
923 void BLI_bvhtree_balance(BVHTree *tree)
924 {
925         int i;
926
927         BVHNode*  branches_array = tree->nodearray + tree->totleaf;
928         BVHNode** leafs_array    = tree->nodes;
929
930         //This function should only be called once (some big bug goes here if its being called more than once per tree)
931         assert(tree->totbranch == 0);
932
933         //Build the implicit tree
934         non_recursive_bvh_div_nodes(tree, branches_array, leafs_array, tree->totleaf);
935
936         //current code expects the branches to be linked to the nodes array
937         //we perform that linkage here
938         tree->totbranch = implicit_needed_branches(tree->tree_type, tree->totleaf);
939         for(i = 0; i < tree->totbranch; i++)
940                 tree->nodes[tree->totleaf + i] = branches_array + i;
941
942         //bvhtree_info(tree);
943 }
944
945 int BLI_bvhtree_insert(BVHTree *tree, int index, float *co, int numpoints)
946 {
947         int i;
948         BVHNode *node = NULL;
949         
950         // insert should only possible as long as tree->totbranch is 0
951         if(tree->totbranch > 0)
952                 return 0;
953         
954         if(tree->totleaf+1 >= MEM_allocN_len(tree->nodes)/sizeof(*(tree->nodes)))
955                 return 0;
956         
957         // TODO check if have enough nodes in array
958         
959         node = tree->nodes[tree->totleaf] = &(tree->nodearray[tree->totleaf]);
960         tree->totleaf++;
961         
962         create_kdop_hull(tree, node, co, numpoints, 0);
963         node->index= index;
964         
965         // inflate the bv with some epsilon
966         for (i = tree->start_axis; i < tree->stop_axis; i++)
967         {
968                 node->bv[(2 * i)] -= tree->epsilon; // minimum 
969                 node->bv[(2 * i) + 1] += tree->epsilon; // maximum 
970         }
971
972         return 1;
973 }
974
975
976 // call before BLI_bvhtree_update_tree()
977 int BLI_bvhtree_update_node(BVHTree *tree, int index, float *co, float *co_moving, int numpoints)
978 {
979         int i;
980         BVHNode *node= NULL;
981         
982         // check if index exists
983         if(index > tree->totleaf)
984                 return 0;
985         
986         node = tree->nodearray + index;
987         
988         create_kdop_hull(tree, node, co, numpoints, 0);
989         
990         if(co_moving)
991                 create_kdop_hull(tree, node, co_moving, numpoints, 1);
992         
993         // inflate the bv with some epsilon
994         for (i = tree->start_axis; i < tree->stop_axis; i++)
995         {
996                 node->bv[(2 * i)] -= tree->epsilon; // minimum 
997                 node->bv[(2 * i) + 1] += tree->epsilon; // maximum 
998         }
999
1000         return 1;
1001 }
1002
1003 // call BLI_bvhtree_update_node() first for every node/point/triangle
1004 void BLI_bvhtree_update_tree(BVHTree *tree)
1005 {
1006         //Update bottom=>top
1007         //TRICKY: the way we build the tree all the childs have an index greater than the parent
1008         //This allows us todo a bottom up update by starting on the biger numbered branch
1009
1010         BVHNode** root  = tree->nodes + tree->totleaf;
1011         BVHNode** index = tree->nodes + tree->totleaf + tree->totbranch-1;
1012
1013         for (; index >= root; index--)
1014                 node_join(tree, *index);
1015 }
1016
1017 float BLI_bvhtree_getepsilon(BVHTree *tree)
1018 {
1019         return tree->epsilon;
1020 }
1021
1022
1023 /*
1024  * BLI_bvhtree_overlap
1025  */
1026 // overlap - is it possbile for 2 bv's to collide ?
1027 static int tree_overlap(BVHNode *node1, BVHNode *node2, int start_axis, int stop_axis)
1028 {
1029         float *bv1 = node1->bv;
1030         float *bv2 = node2->bv;
1031
1032         float *bv1_end = bv1 + (stop_axis<<1);
1033                 
1034         bv1 += start_axis<<1;
1035         bv2 += start_axis<<1;
1036         
1037         // test all axis if min + max overlap
1038         for (; bv1 != bv1_end; bv1+=2, bv2+=2)
1039         {
1040                 if ((*(bv1) > *(bv2 + 1)) || (*(bv2) > *(bv1 + 1))) 
1041                         return 0;
1042         }
1043         
1044         return 1;
1045 }
1046
1047 static void traverse(BVHOverlapData *data, BVHNode *node1, BVHNode *node2)
1048 {
1049         int j;
1050         
1051         if(tree_overlap(node1, node2, data->start_axis, data->stop_axis))
1052         {
1053                 // check if node1 is a leaf
1054                 if(!node1->totnode)
1055                 {
1056                         // check if node2 is a leaf
1057                         if(!node2->totnode)
1058                         {
1059                                 
1060                                 if(node1 == node2)
1061                                 {
1062                                         return;
1063                                 }
1064                                         
1065                                 if(data->i >= data->max_overlap)
1066                                 {       
1067                                         // try to make alloc'ed memory bigger
1068                                         data->overlap = realloc(data->overlap, sizeof(BVHTreeOverlap)*data->max_overlap*2);
1069                                         
1070                                         if(!data->overlap)
1071                                         {
1072                                                 printf("Out of Memory in traverse\n");
1073                                                 return;
1074                                         }
1075                                         data->max_overlap *= 2;
1076                                 }
1077                                 
1078                                 // both leafs, insert overlap!
1079                                 data->overlap[data->i].indexA = node1->index;
1080                                 data->overlap[data->i].indexB = node2->index;
1081
1082                                 data->i++;
1083                         }
1084                         else
1085                         {
1086                                 for(j = 0; j < data->tree2->tree_type; j++)
1087                                 {
1088                                         if(node2->children[j])
1089                                                 traverse(data, node1, node2->children[j]);
1090                                 }
1091                         }
1092                 }
1093                 else
1094                 {
1095                         
1096                         for(j = 0; j < data->tree2->tree_type; j++)
1097                         {
1098                                 if(node1->children[j])
1099                                         traverse(data, node1->children[j], node2);
1100                         }
1101                 }
1102         }
1103         return;
1104 }
1105
1106 BVHTreeOverlap *BLI_bvhtree_overlap(BVHTree *tree1, BVHTree *tree2, int *result)
1107 {
1108         int j, total = 0;
1109         BVHTreeOverlap *overlap = NULL, *to = NULL;
1110         BVHOverlapData **data;
1111         
1112         // check for compatibility of both trees (can't compare 14-DOP with 18-DOP)
1113         if((tree1->axis != tree2->axis) && (tree1->axis == 14 || tree2->axis == 14) && (tree1->axis == 18 || tree2->axis == 18))
1114                 return 0;
1115         
1116         // fast check root nodes for collision before doing big splitting + traversal
1117         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)))
1118                 return 0;
1119
1120         data = MEM_callocN(sizeof(BVHOverlapData *)* tree1->tree_type, "BVHOverlapData_star");
1121         
1122         for(j = 0; j < tree1->tree_type; j++)
1123         {
1124                 data[j] = (BVHOverlapData *)MEM_callocN(sizeof(BVHOverlapData), "BVHOverlapData");
1125                 
1126                 // init BVHOverlapData
1127                 data[j]->overlap = (BVHTreeOverlap *)malloc(sizeof(BVHTreeOverlap)*MAX2(tree1->totleaf, tree2->totleaf));
1128                 data[j]->tree1 = tree1;
1129                 data[j]->tree2 = tree2;
1130                 data[j]->max_overlap = MAX2(tree1->totleaf, tree2->totleaf);
1131                 data[j]->i = 0;
1132                 data[j]->start_axis = MIN2(tree1->start_axis, tree2->start_axis);
1133                 data[j]->stop_axis  = MIN2(tree1->stop_axis,  tree2->stop_axis );
1134         }
1135
1136 #pragma omp parallel for private(j) schedule(static)
1137         for(j = 0; j < MIN2(tree1->tree_type, tree1->nodes[tree1->totleaf]->totnode); j++)
1138         {
1139                 traverse(data[j], tree1->nodes[tree1->totleaf]->children[j], tree2->nodes[tree2->totleaf]);
1140         }
1141         
1142         for(j = 0; j < tree1->tree_type; j++)
1143                 total += data[j]->i;
1144         
1145         to = overlap = (BVHTreeOverlap *)MEM_callocN(sizeof(BVHTreeOverlap)*total, "BVHTreeOverlap");
1146         
1147         for(j = 0; j < tree1->tree_type; j++)
1148         {
1149                 memcpy(to, data[j]->overlap, data[j]->i*sizeof(BVHTreeOverlap));
1150                 to+=data[j]->i;
1151         }
1152         
1153         for(j = 0; j < tree1->tree_type; j++)
1154         {
1155                 free(data[j]->overlap);
1156                 MEM_freeN(data[j]);
1157         }
1158         MEM_freeN(data);
1159         
1160         (*result) = total;
1161         return overlap;
1162 }
1163
1164
1165 /*
1166  * Nearest neighbour - BLI_bvhtree_find_nearest
1167  */
1168 static float squared_dist(const float *a, const float *b)
1169 {
1170         float tmp[3];
1171         VECSUB(tmp, a, b);
1172         return INPR(tmp, tmp);
1173 }
1174
1175 //Determines the nearest point of the given node BV. Returns the squared distance to that point.
1176 static float calc_nearest_point(const float *proj, BVHNode *node, float *nearest)
1177 {
1178         int i;
1179         const float *bv = node->bv;
1180
1181         //nearest on AABB hull
1182         for(i=0; i != 3; i++, bv += 2)
1183         {
1184                 if(bv[0] > proj[i])
1185                         nearest[i] = bv[0];
1186                 else if(bv[1] < proj[i])
1187                         nearest[i] = bv[1];
1188                 else
1189                         nearest[i] = proj[i]; 
1190         }
1191
1192 /*
1193         //nearest on a general hull
1194         VECCOPY(nearest, data->co);
1195         for(i = data->tree->start_axis; i != data->tree->stop_axis; i++, bv+=2)
1196         {
1197                 float proj = INPR( nearest, KDOP_AXES[i]);
1198                 float dl = bv[0] - proj;
1199                 float du = bv[1] - proj;
1200
1201                 if(dl > 0)
1202                 {
1203                         VECADDFAC(nearest, nearest, KDOP_AXES[i], dl);
1204                 }
1205                 else if(du < 0)
1206                 {
1207                         VECADDFAC(nearest, nearest, KDOP_AXES[i], du);
1208                 }
1209         }
1210 */
1211         return squared_dist(proj, nearest);
1212 }
1213
1214
1215 typedef struct NodeDistance
1216 {
1217         BVHNode *node;
1218         float dist;
1219
1220 } NodeDistance;
1221
1222 #define NodeDistance_priority(a,b)      ( (a).dist < (b).dist )
1223
1224 // TODO: use a priority queue to reduce the number of nodes looked on
1225 static void dfs_find_nearest_dfs(BVHNearestData *data, BVHNode *node)
1226 {
1227         if(node->totnode == 0)
1228         {
1229                 if(data->callback)
1230                         data->callback(data->userdata , node->index, data->co, &data->nearest);
1231                 else
1232                 {
1233                         data->nearest.index     = node->index;
1234                         data->nearest.dist      = calc_nearest_point(data->proj, node, data->nearest.co);
1235                 }
1236         }
1237         else
1238         {
1239                 //Better heuristic to pick the closest node to dive on
1240                 int i;
1241                 float nearest[3];
1242
1243                 if(data->proj[ node->main_axis ] <= node->children[0]->bv[node->main_axis*2+1])
1244                 {
1245
1246                         for(i=0; i != node->totnode; i++)
1247                         {
1248                                 if( calc_nearest_point(data->proj, node->children[i], nearest) >= data->nearest.dist) continue;
1249                                 dfs_find_nearest_dfs(data, node->children[i]);
1250                         }
1251                 }
1252                 else
1253                 {
1254                         for(i=node->totnode-1; i >= 0 ; i--)
1255                         {
1256                                 if( calc_nearest_point(data->proj, node->children[i], nearest) >= data->nearest.dist) continue;
1257                                 dfs_find_nearest_dfs(data, node->children[i]);
1258                         }
1259                 }
1260         }
1261 }
1262
1263 static void dfs_find_nearest_begin(BVHNearestData *data, BVHNode *node)
1264 {
1265         float nearest[3], sdist;
1266         sdist = calc_nearest_point(data->proj, node, nearest);
1267         if(sdist >= data->nearest.dist) return;
1268         dfs_find_nearest_dfs(data, node);
1269 }
1270
1271
1272 #if 0
1273 static void NodeDistance_push_heap(NodeDistance *heap, int heap_size)
1274 PUSH_HEAP_BODY(NodeDistance, NodeDistance_priority, heap, heap_size)
1275
1276 static void NodeDistance_pop_heap(NodeDistance *heap, int heap_size)
1277 POP_HEAP_BODY(NodeDistance, NodeDistance_priority, heap, heap_size)
1278
1279 //NN function that uses an heap.. this functions leads to an optimal number of min-distance
1280 //but for normal tri-faces and BV 6-dop.. a simple dfs with local heuristics (as implemented
1281 //in source/blender/blenkernel/intern/shrinkwrap.c) works faster.
1282 //
1283 //It may make sense to use this function if the callback queries are very slow.. or if its impossible
1284 //to get a nice heuristic
1285 //
1286 //this function uses "malloc/free" instead of the MEM_* because it intends to be openmp safe
1287 static void bfs_find_nearest(BVHNearestData *data, BVHNode *node)
1288 {
1289         int i;
1290         NodeDistance default_heap[DEFAULT_FIND_NEAREST_HEAP_SIZE];
1291         NodeDistance *heap=default_heap, current;
1292         int heap_size = 0, max_heap_size = sizeof(default_heap)/sizeof(default_heap[0]);
1293         float nearest[3];
1294
1295         int callbacks = 0, push_heaps = 0;
1296
1297         if(node->totnode == 0)
1298         {
1299                 dfs_find_nearest_dfs(data, node);
1300                 return;
1301         }
1302
1303         current.node = node;
1304         current.dist = calc_nearest_point(data->proj, node, nearest);
1305
1306         while(current.dist < data->nearest.dist)
1307         {
1308 //              printf("%f : %f\n", current.dist, data->nearest.dist);
1309                 for(i=0; i< current.node->totnode; i++)
1310                 {
1311                         BVHNode *child = current.node->children[i];
1312                         if(child->totnode == 0)
1313                         {
1314                                 callbacks++;
1315                                 dfs_find_nearest_dfs(data, child);
1316                         }
1317                         else
1318                         {
1319                                 //adjust heap size
1320                                 if(heap_size >= max_heap_size
1321                                 && ADJUST_MEMORY(default_heap, (void**)&heap, heap_size+1, &max_heap_size, sizeof(heap[0])) == FALSE)
1322                                 {
1323                                         printf("WARNING: bvh_find_nearest got out of memory\n");
1324
1325                                         if(heap != default_heap)
1326                                                 free(heap);
1327
1328                                         return;
1329                                 }
1330
1331                                 heap[heap_size].node = current.node->children[i];
1332                                 heap[heap_size].dist = calc_nearest_point(data->proj, current.node->children[i], nearest);
1333
1334                                 if(heap[heap_size].dist >= data->nearest.dist) continue;
1335                                 heap_size++;
1336
1337                                 NodeDistance_push_heap(heap, heap_size);
1338         //                      PUSH_HEAP_BODY(NodeDistance, NodeDistance_priority, heap, heap_size);
1339                                 push_heaps++;
1340                         }
1341                 }
1342                 
1343                 if(heap_size == 0) break;
1344
1345                 current = heap[0];
1346                 NodeDistance_pop_heap(heap, heap_size);
1347 //              POP_HEAP_BODY(NodeDistance, NodeDistance_priority, heap, heap_size);
1348                 heap_size--;
1349         }
1350
1351 //      printf("hsize=%d, callbacks=%d, pushs=%d\n", heap_size, callbacks, push_heaps);
1352
1353         if(heap != default_heap)
1354                 free(heap);
1355 }
1356 #endif
1357
1358
1359 int BLI_bvhtree_find_nearest(BVHTree *tree, const float *co, BVHTreeNearest *nearest, BVHTree_NearestPointCallback callback, void *userdata)
1360 {
1361         int i;
1362
1363         BVHNearestData data;
1364         BVHNode* root = tree->nodes[tree->totleaf];
1365
1366         //init data to search
1367         data.tree = tree;
1368         data.co = co;
1369
1370         data.callback = callback;
1371         data.userdata = userdata;
1372
1373         for(i = data.tree->start_axis; i != data.tree->stop_axis; i++)
1374         {
1375                 data.proj[i] = INPR(data.co, KDOP_AXES[i]);
1376         }
1377
1378         if(nearest)
1379         {
1380                 memcpy( &data.nearest , nearest, sizeof(*nearest) );
1381         }
1382         else
1383         {
1384                 data.nearest.index = -1;
1385                 data.nearest.dist = FLT_MAX;
1386         }
1387
1388         //dfs search
1389         if(root)
1390                 dfs_find_nearest_begin(&data, root);
1391
1392         //copy back results
1393         if(nearest)
1394         {
1395                 memcpy(nearest, &data.nearest, sizeof(*nearest));
1396         }
1397
1398         return data.nearest.index;
1399 }
1400
1401
1402 /*
1403  * Raycast - BLI_bvhtree_ray_cast
1404  *
1405  * raycast is done by performing a DFS on the BVHTree and saving the closest hit
1406  */
1407
1408 //Determines the distance that the ray must travel to hit the bounding volume of the given node
1409 static float ray_nearest_hit(BVHRayCastData *data, float *bv)
1410 {
1411         int i;
1412
1413         float low = 0, upper = data->hit.dist;
1414
1415         for(i=0; i != 3; i++, bv += 2)
1416         {
1417                 if(data->ray_dot_axis[i] == 0.0f)
1418                 {
1419                         //axis aligned ray
1420                         if(data->ray.origin[i] < bv[0] - data->ray.radius
1421                         || data->ray.origin[i] > bv[1] + data->ray.radius)
1422                                 return FLT_MAX;
1423                 }
1424                 else
1425                 {
1426                         float ll = (bv[0] - data->ray.radius - data->ray.origin[i]) / data->ray_dot_axis[i];
1427                         float lu = (bv[1] + data->ray.radius - data->ray.origin[i]) / data->ray_dot_axis[i];
1428
1429                         if(data->ray_dot_axis[i] > 0.0f)
1430                         {
1431                                 if(ll > low)   low = ll;
1432                                 if(lu < upper) upper = lu;
1433                         }
1434                         else
1435                         {
1436                                 if(lu > low)   low = lu;
1437                                 if(ll < upper) upper = ll;
1438                         }
1439         
1440                         if(low > upper) return FLT_MAX;
1441                 }
1442         }
1443         return low;
1444 }
1445
1446 static void dfs_raycast(BVHRayCastData *data, BVHNode *node)
1447 {
1448         int i;
1449
1450         //ray-bv is really fast.. and simple tests revealed its worth to test it
1451         //before calling the ray-primitive functions
1452         float dist = ray_nearest_hit(data, node->bv);
1453         if(dist >= data->hit.dist) return;
1454
1455         if(node->totnode == 0)
1456         {
1457                 if(data->callback)
1458                         data->callback(data->userdata, node->index, &data->ray, &data->hit);
1459                 else
1460                 {
1461                         data->hit.index = node->index;
1462                         data->hit.dist  = dist;
1463                         VECADDFAC(data->hit.co, data->ray.origin, data->ray.direction, dist);
1464                 }
1465         }
1466         else
1467         {
1468                 //pick loop direction to dive into the tree (based on ray direction and split axis)
1469                 if(data->ray_dot_axis[ (int)node->main_axis ] > 0.0f)
1470                 {
1471                         for(i=0; i != node->totnode; i++)
1472                         {
1473                                 dfs_raycast(data, node->children[i]);
1474                         }
1475                 }
1476                 else
1477                 {
1478                         for(i=node->totnode-1; i >= 0; i--)
1479                         {
1480                                 dfs_raycast(data, node->children[i]);
1481                         }
1482                 }
1483         }
1484 }
1485
1486 int BLI_bvhtree_ray_cast(BVHTree *tree, const float *co, const float *dir, float radius, BVHTreeRayHit *hit, BVHTree_RayCastCallback callback, void *userdata)
1487 {
1488         int i;
1489         BVHRayCastData data;
1490         BVHNode * root = tree->nodes[tree->totleaf];
1491
1492         data.tree = tree;
1493
1494         data.callback = callback;
1495         data.userdata = userdata;
1496
1497         VECCOPY(data.ray.origin,    co);
1498         VECCOPY(data.ray.direction, dir);
1499         data.ray.radius = radius;
1500
1501         Normalize(data.ray.direction);
1502
1503         for(i=0; i<3; i++)
1504         {
1505                 data.ray_dot_axis[i] = INPR( data.ray.direction, KDOP_AXES[i]);
1506
1507                 if(fabs(data.ray_dot_axis[i]) < FLT_EPSILON)
1508                         data.ray_dot_axis[i] = 0.0;
1509         }
1510
1511
1512         if(hit)
1513                 memcpy( &data.hit, hit, sizeof(*hit) );
1514         else
1515         {
1516                 data.hit.index = -1;
1517                 data.hit.dist = FLT_MAX;
1518         }
1519
1520         if(root)
1521                 dfs_raycast(&data, root);
1522
1523
1524         if(hit)
1525                 memcpy( hit, &data.hit, sizeof(*hit) );
1526
1527         return data.hit.index;
1528 }
1529
1530 float BLI_bvhtree_bb_raycast(float *bv, float *light_start, float *light_end, float *pos)
1531 {
1532         BVHRayCastData data;
1533         float dist = 0.0;
1534
1535         data.hit.dist = FLT_MAX;
1536         
1537         // get light direction
1538         data.ray.direction[0] = light_end[0] - light_start[0];
1539         data.ray.direction[1] = light_end[1] - light_start[1];
1540         data.ray.direction[2] = light_end[2] - light_start[2];
1541         
1542         data.ray.radius = 0.0;
1543         
1544         data.ray.origin[0] = light_start[0];
1545         data.ray.origin[1] = light_start[1];
1546         data.ray.origin[2] = light_start[2];
1547         
1548         Normalize(data.ray.direction);
1549         VECCOPY(data.ray_dot_axis, data.ray.direction);
1550         
1551         dist = ray_nearest_hit(&data, bv);
1552         
1553         if(dist > 0.0)
1554         {
1555                 VECADDFAC(pos, light_start, data.ray.direction, dist);
1556         }
1557         return dist;
1558         
1559 }
1560
1561 /*
1562  * Range Query - as request by broken :P
1563  *
1564  * Allocs and fills an array with the indexs of node that are on the given spherical range (center, radius) 
1565  * Returns the size of the array.
1566  */
1567 typedef struct RangeQueryData
1568 {
1569         BVHTree *tree;
1570         const float *center;
1571         float radius;                   //squared radius
1572
1573         int hits;
1574
1575         BVHTree_RangeQuery callback;
1576         void *userdata;
1577
1578
1579 } RangeQueryData;
1580
1581
1582 static void dfs_range_query(RangeQueryData *data, BVHNode *node)
1583 {
1584         if(node->totnode == 0)
1585         {
1586
1587                 //Calculate the node min-coords (if the node was a point then this is the point coordinates)
1588                 float co[3];
1589                 co[0] = node->bv[0];
1590                 co[1] = node->bv[2];
1591                 co[2] = node->bv[4];
1592
1593         }
1594         else
1595         {
1596                 int i;
1597                 for(i=0; i != node->totnode; i++)
1598                 {
1599                         float nearest[3];
1600                         float dist = calc_nearest_point(data->center, node->children[i], nearest);
1601                         if(dist < data->radius)
1602                         {
1603                                 //Its a leaf.. call the callback
1604                                 if(node->children[i]->totnode == 0)
1605                                 {
1606                                         data->hits++;
1607                                         data->callback( data->userdata, node->children[i]->index, dist );
1608                                 }
1609                                 else
1610                                         dfs_range_query( data, node->children[i] );
1611                         }
1612                 }
1613         }
1614 }
1615
1616 int BLI_bvhtree_range_query(BVHTree *tree, const float *co, float radius, BVHTree_RangeQuery callback, void *userdata)
1617 {
1618         BVHNode * root = tree->nodes[tree->totleaf];
1619
1620         RangeQueryData data;
1621         data.tree = tree;
1622         data.center = co;
1623         data.radius = radius*radius;
1624         data.hits = 0;
1625
1626         data.callback = callback;
1627         data.userdata = userdata;
1628
1629         if(root != NULL)
1630         {
1631                 float nearest[3];
1632                 float dist = calc_nearest_point(data.center, root, nearest);
1633                 if(dist < data.radius)
1634                 {
1635                         //Its a leaf.. call the callback
1636                         if(root->totnode == 0)
1637                         {
1638                                 data.hits++;
1639                                 data.callback( data.userdata, root->index, dist );
1640                         }
1641                         else
1642                                 dfs_range_query( &data, root );
1643                 }
1644         }
1645
1646         return data.hits;
1647 }