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