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