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