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  *  \brief BVH-tree implementation.
  31  *
  32  * k-DOP BVH (Discrete Oriented Polytope, Bounding Volume Hierarchy).
  33  * A k-DOP is represented as k/2 pairs of min , max values for k/2 directions (intervals, "slabs").
  34  *
  35  * See: http://www.gris.uni-tuebingen.de/people/staff/jmezger/papers/bvh.pdf
  36  *
  37  * implements a bvh-tree structure with support for:
  38  *
  39  * - Ray-cast:
  40  *   #BLI_bvhtree_ray_cast, #BVHRayCastData
  41  * - Nearest point on surface:
  42  *   #BLI_bvhtree_find_nearest, #BVHNearestData
  43  * - Overlapping 2 trees:
  44  *   #BLI_bvhtree_overlap, #BVHOverlapData_Shared, #BVHOverlapData_Thread
  45  * - Range Query:
  46  *   #BLI_bvhtree_range_query
  47  */
  48
  49 #include <assert.h>
  50
  51 #include "MEM_guardedalloc.h"
  52
  53 #include "BLI_utildefines.h"
  54 #include "BLI_alloca.h"
  55 #include "BLI_stack.h"
  56 #include "BLI_kdopbvh.h"
  57 #include "BLI_math.h"
  58 #include "BLI_task.h"
  59
  60 #include "BLI_strict_flags.h"
  61
  62 /* used for iterative_raycast */
  63 // #define USE_SKIP_LINKS
  64
  65 /* Use to print balanced output. */
  66 // #define USE_PRINT_TREE
  67
  68 /* Check tree is valid. */
  69 // #define USE_VERIFY_TREE
  70
  71
  72 #define MAX_TREETYPE 32
  73
  74 /* Setting zero so we can catch bugs in BLI_task/KDOPBVH.
  75  * TODO(sergey): Deduplicate the limits with PBVH from BKE.
  76  */
  77 #ifdef DEBUG
  78 #  define KDOPBVH_THREAD_LEAF_THRESHOLD 0
  79 #else
  80 #  define KDOPBVH_THREAD_LEAF_THRESHOLD 1024
  81 #endif
  82
  83
  84 /* -------------------------------------------------------------------- */
  85
  86 /** \name Struct Definitions
  87  * \{ */
  88
  89 typedef unsigned char axis_t;
  90
  91 typedef struct BVHNode {
  92         struct BVHNode **children;
  93         struct BVHNode *parent; /* some user defined traversed need that */
  94 #ifdef USE_SKIP_LINKS
  95         struct BVHNode *skip[2];
  96 #endif
  97         float *bv;      /* Bounding volume of all nodes, max 13 axis */
  98         int index;      /* face, edge, vertex index */
  99         char totnode;   /* how many nodes are used, used for speedup */
 100         char main_axis; /* Axis used to split this node */
 101 } BVHNode;
 102
 103 /* keep under 26 bytes for speed purposes */
 104 struct BVHTree {
 105         BVHNode **nodes;
 106         BVHNode *nodearray;     /* pre-alloc branch nodes */
 107         BVHNode **nodechild;    /* pre-alloc childs for nodes */
 108         float   *nodebv;        /* pre-alloc bounding-volumes for nodes */
 109         float epsilon;          /* epslion is used for inflation of the k-dop      */
 110         int totleaf;            /* leafs */
 111         int totbranch;
 112         axis_t start_axis, stop_axis;  /* bvhtree_kdop_axes array indices according to axis */
 113         axis_t axis;                   /* kdop type (6 => OBB, 7 => AABB, ...) */
 114         char tree_type;                /* type of tree (4 => quadtree) */
 115 };
 116
 117 /* optimization, ensure we stay small */
 118 BLI_STATIC_ASSERT((sizeof(void *) == 8 && sizeof(BVHTree) <= 48) ||
 119                   (sizeof(void *) == 4 && sizeof(BVHTree) <= 32),
 120                   "over sized")
 121
 122 /* avoid duplicating vars in BVHOverlapData_Thread */
 123 typedef struct BVHOverlapData_Shared {
 124         const BVHTree *tree1, *tree2;
 125         axis_t start_axis, stop_axis;
 126
 127         /* use for callbacks */
 128         BVHTree_OverlapCallback callback;
 129         void *userdata;
 130 } BVHOverlapData_Shared;
 131
 132 typedef struct BVHOverlapData_Thread {
 133         BVHOverlapData_Shared *shared;
 134         struct BLI_Stack *overlap;  /* store BVHTreeOverlap */
 135         /* use for callbacks */
 136         int thread;
 137 } BVHOverlapData_Thread;
 138
 139 typedef struct BVHNearestData {
 140         const BVHTree *tree;
 141         const float *co;
 142         BVHTree_NearestPointCallback callback;
 143         void    *userdata;
 144         float proj[13];         /* coordinates projection over axis */
 145         BVHTreeNearest nearest;
 146
 147 } BVHNearestData;
 148
 149 typedef struct BVHRayCastData {
 150         const BVHTree *tree;
 151
 152         BVHTree_RayCastCallback callback;
 153         void    *userdata;
 154
 155
 156         BVHTreeRay ray;
 157
 158 #ifdef USE_KDOPBVH_WATERTIGHT
 159         struct IsectRayPrecalc isect_precalc;
 160 #endif
 161
 162         /* initialized by bvhtree_ray_cast_data_precalc */
 163         float ray_dot_axis[13];
 164         float idot_axis[13];
 165         int index[6];
 166
 167         BVHTreeRayHit hit;
 168 } BVHRayCastData;
 169
 170 typedef struct BVHNearestProjectedData {
 171         const BVHTree *tree;
 172         struct DistProjectedAABBPrecalc precalc;
 173         bool closest_axis[3];
 174         float clip_plane[6][4];
 175         int clip_plane_len;
 176         BVHTree_NearestProjectedCallback callback;
 177         void *userdata;
 178         BVHTreeNearest nearest;
 179
 180 } BVHNearestProjectedData;
 181
 182 /** \} */
 183
 184
 185 /**
 186  * Bounding Volume Hierarchy Definition
 187  *
 188  * Notes: From OBB until 26-DOP --> all bounding volumes possible, just choose type below
 189  * Notes: You have to choose the type at compile time ITM
 190  * Notes: You can choose the tree type --> binary, quad, octree, choose below
 191  */
 192
 193 const float bvhtree_kdop_axes[13][3] = {
 194         {1.0, 0, 0}, {0, 1.0, 0}, {0, 0, 1.0},
 195         {1.0, 1.0, 1.0}, {1.0, -1.0, 1.0}, {1.0, 1.0, -1.0}, {1.0, -1.0, -1.0},
 196         {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}, {0, 1.0, -1.0}
 197 };
 198
 199
 200 /* -------------------------------------------------------------------- */
 201
 202 /** \name Utility Functions
 203  * \{ */
 204
 205 MINLINE axis_t min_axis(axis_t a, axis_t b)
 206 {
 207         return (a < b) ? a : b;
 208 }
 209 #if 0
 210 MINLINE axis_t max_axis(axis_t a, axis_t b)
 211 {
 212         return (b < a) ? a : b;
 213 }
 214 #endif
 215
 216 #if 0
 217
 218 /*
 219  * Generic push and pop heap
 220  */
 221 #define PUSH_HEAP_BODY(HEAP_TYPE, PRIORITY, heap, heap_size)                  \
 222         {                                                                         \
 223                 HEAP_TYPE element = heap[heap_size - 1];                              \
 224                 int child = heap_size - 1;                                            \
 225                 while (child != 0) {                                                  \
 226                         int parent = (child - 1) / 2;                                     \
 227                         if (PRIORITY(element, heap[parent])) {                            \
 228                                 heap[child] = heap[parent];                                   \
 229                                 child = parent;                                               \
 230                         }                                                                 \
 231                         else {                                                            \
 232                                 break;                                                        \
 233                         }                                                                 \
 234                 }                                                                     \
 235                 heap[child] = element;                                                \
 236         } (void)0
 237
 238 #define POP_HEAP_BODY(HEAP_TYPE, PRIORITY, heap, heap_size)                   \
 239         {                                                                         \
 240                 HEAP_TYPE element = heap[heap_size - 1];                              \
 241                 int parent = 0;                                                       \
 242                 while (parent < (heap_size - 1) / 2) {                                \
 243                         int child2 = (parent + 1) * 2;                                    \
 244                         if (PRIORITY(heap[child2 - 1], heap[child2])) {                   \
 245                                 child2--;                                                     \
 246                         }                                                                 \
 247                         if (PRIORITY(element, heap[child2])) {                            \
 248                                 break;                                                        \
 249                         }                                                                 \
 250                         heap[parent] = heap[child2];                                      \
 251                         parent = child2;                                                  \
 252                 }                                                                     \
 253                 heap[parent] = element;                                               \
 254         } (void)0
 255
 256 static bool ADJUST_MEMORY(void *local_memblock, void **memblock, int new_size, int *max_size, int size_per_item)
 257 {
 258         int new_max_size = *max_size * 2;
 259         void *new_memblock = NULL;
 260
 261         if (new_size <= *max_size) {
 262                 return true;
 263         }
 264
 265         if (*memblock == local_memblock) {
 266                 new_memblock = malloc(size_per_item * new_max_size);
 267                 memcpy(new_memblock, *memblock, size_per_item * *max_size);
 268         }
 269         else {
 270                 new_memblock = realloc(*memblock, size_per_item * new_max_size);
 271         }
 272
 273         if (new_memblock) {
 274                 *memblock = new_memblock;
 275                 *max_size = new_max_size;
 276                 return true;
 277         }
 278         else {
 279                 return false;
 280         }
 281 }
 282 #endif
 283
 284 /**
 285  * Introsort
 286  * with permission deriven from the following Java code:
 287  * http://ralphunden.net/content/tutorials/a-guide-to-introsort/
 288  * and he derived it from the SUN STL
 289  */
 290
 291 //static int size_threshold = 16;
 292
 293 #if 0
 294 /**
 295  * Common methods for all algorithms
 296  */
 297 static int floor_lg(int a)
 298 {
 299         return (int)(floor(log(a) / log(2)));
 300 }
 301 #endif
 302
 303 static void node_minmax_init(const BVHTree *tree, BVHNode *node)
 304 {
 305         axis_t axis_iter;
 306         float (*bv)[2] = (float (*)[2])node->bv;
 307
 308         for (axis_iter = tree->start_axis; axis_iter != tree->stop_axis; axis_iter++) {
 309                 bv[axis_iter][0] =  FLT_MAX;
 310                 bv[axis_iter][1] = -FLT_MAX;
 311         }
 312 }
 313
 314 /** \} */
 315
 316
 317 /* -------------------------------------------------------------------- */
 318
 319 /** \name Balance Utility Functions
 320  * \{ */
 321
 322 /**
 323  * Insertion sort algorithm
 324  */
 325 static void bvh_insertionsort(BVHNode **a, int lo, int hi, int axis)
 326 {
 327         int i, j;
 328         BVHNode *t;
 329         for (i = lo; i < hi; i++) {
 330                 j = i;
 331                 t = a[i];
 332                 while ((j != lo) && (t->bv[axis] < (a[j - 1])->bv[axis])) {
 333                         a[j] = a[j - 1];
 334                         j--;
 335                 }
 336                 a[j] = t;
 337         }
 338 }
 339
 340 static int bvh_partition(BVHNode **a, int lo, int hi, BVHNode *x, int axis)
 341 {
 342         int i = lo, j = hi;
 343         while (1) {
 344                 while (a[i]->bv[axis] < x->bv[axis]) {
 345                         i++;
 346                 }
 347                 j--;
 348                 while (x->bv[axis] < a[j]->bv[axis]) {
 349                         j--;
 350                 }
 351                 if (!(i < j)) {
 352                         return i;
 353                 }
 354                 SWAP(BVHNode *, a[i], a[j]);
 355                 i++;
 356         }
 357 }
 358
 359 #if 0
 360 /**
 361  * Heapsort algorithm
 362  */
 363 static void bvh_downheap(BVHNode **a, int i, int n, int lo, int axis)
 364 {
 365         BVHNode *d = a[lo + i - 1];
 366         int child;
 367         while (i <= n / 2) {
 368                 child = 2 * i;
 369                 if ((child < n) && ((a[lo + child - 1])->bv[axis] < (a[lo + child])->bv[axis])) {
 370                         child++;
 371                 }
 372                 if (!(d->bv[axis] < (a[lo + child - 1])->bv[axis])) break;
 373                 a[lo + i - 1] = a[lo + child - 1];
 374                 i = child;
 375         }
 376         a[lo + i - 1] = d;
 377 }
 378
 379 static void bvh_heapsort(BVHNode **a, int lo, int hi, int axis)
 380 {
 381         int n = hi - lo, i;
 382         for (i = n / 2; i >= 1; i = i - 1) {
 383                 bvh_downheap(a, i, n, lo, axis);
 384         }
 385         for (i = n; i > 1; i = i - 1) {
 386                 SWAP(BVHNode *, a[lo], a[lo + i - 1]);
 387                 bvh_downheap(a, 1, i - 1, lo, axis);
 388         }
 389 }
 390 #endif
 391
 392 static BVHNode *bvh_medianof3(BVHNode **a, int lo, int mid, int hi, int axis)  /* returns Sortable */
 393 {
 394         if ((a[mid])->bv[axis] < (a[lo])->bv[axis]) {
 395                 if ((a[hi])->bv[axis] < (a[mid])->bv[axis])
 396                         return a[mid];
 397                 else {
 398                         if ((a[hi])->bv[axis] < (a[lo])->bv[axis])
 399                                 return a[hi];
 400                         else
 401                                 return a[lo];
 402                 }
 403         }
 404         else {
 405                 if ((a[hi])->bv[axis] < (a[mid])->bv[axis]) {
 406                         if ((a[hi])->bv[axis] < (a[lo])->bv[axis])
 407                                 return a[lo];
 408                         else
 409                                 return a[hi];
 410                 }
 411                 else
 412                         return a[mid];
 413         }
 414 }
 415
 416 #if 0
 417 /*
 418  * Quicksort algorithm modified for Introsort
 419  */
 420 static void bvh_introsort_loop(BVHNode **a, int lo, int hi, int depth_limit, int axis)
 421 {
 422         int p;
 423
 424         while (hi - lo > size_threshold) {
 425                 if (depth_limit == 0) {
 426                         bvh_heapsort(a, lo, hi, axis);
 427                         return;
 428                 }
 429                 depth_limit = depth_limit - 1;
 430                 p = bvh_partition(a, lo, hi, bvh_medianof3(a, lo, lo + ((hi - lo) / 2) + 1, hi - 1, axis), axis);
 431                 bvh_introsort_loop(a, p, hi, depth_limit, axis);
 432                 hi = p;
 433         }
 434 }
 435
 436 static void sort(BVHNode **a0, int begin, int end, int axis)
 437 {
 438         if (begin < end) {
 439                 BVHNode **a = a0;
 440                 bvh_introsort_loop(a, begin, end, 2 * floor_lg(end - begin), axis);
 441                 bvh_insertionsort(a, begin, end, axis);
 442         }
 443 }
 444
 445 static void sort_along_axis(BVHTree *tree, int start, int end, int axis)
 446 {
 447         sort(tree->nodes, start, end, axis);
 448 }
 449 #endif
 450
 451 /**
 452  * \note after a call to this function you can expect one of:
 453  * - every node to left of a[n] are smaller or equal to it
 454  * - every node to the right of a[n] are greater or equal to it */
 455 static void partition_nth_element(BVHNode **a, int begin, int end, const int n, const int axis)
 456 {
 457         while (end - begin > 3) {
 458                 const int cut = bvh_partition(a, begin, end, bvh_medianof3(a, begin, (begin + end) / 2, end - 1, axis), axis);
 459                 if (cut <= n) {
 460                         begin = cut;
 461                 }
 462                 else {
 463                         end = cut;
 464                 }
 465         }
 466         bvh_insertionsort(a, begin, end, axis);
 467 }
 468
 469 #ifdef USE_SKIP_LINKS
 470 static void build_skip_links(BVHTree *tree, BVHNode *node, BVHNode *left, BVHNode *right)
 471 {
 472         int i;
 473
 474         node->skip[0] = left;
 475         node->skip[1] = right;
 476
 477         for (i = 0; i < node->totnode; i++) {
 478                 if (i + 1 < node->totnode)
 479                         build_skip_links(tree, node->children[i], left, node->children[i + 1]);
 480                 else
 481                         build_skip_links(tree, node->children[i], left, right);
 482
 483                 left = node->children[i];
 484         }
 485 }
 486 #endif
 487
 488 /*
 489  * BVHTree bounding volumes functions
 490  */
 491 static void create_kdop_hull(const BVHTree *tree, BVHNode *node, const float *co, int numpoints, int moving)
 492 {
 493         float newminmax;
 494         float *bv = node->bv;
 495         int k;
 496         axis_t axis_iter;
 497
 498         /* don't init boudings for the moving case */
 499         if (!moving) {
 500                 node_minmax_init(tree, node);
 501         }
 502
 503         for (k = 0; k < numpoints; k++) {
 504                 /* for all Axes. */
 505                 for (axis_iter = tree->start_axis; axis_iter < tree->stop_axis; axis_iter++) {
 506                         newminmax = dot_v3v3(&co[k * 3], bvhtree_kdop_axes[axis_iter]);
 507                         if (newminmax < bv[2 * axis_iter])
 508                                 bv[2 * axis_iter] = newminmax;
 509                         if (newminmax > bv[(2 * axis_iter) + 1])
 510                                 bv[(2 * axis_iter) + 1] = newminmax;
 511                 }
 512         }
 513 }
 514
 515 /**
 516  * \note depends on the fact that the BVH's for each face is already built
 517  */
 518 static void refit_kdop_hull(const BVHTree *tree, BVHNode *node, int start, int end)
 519 {
 520         float newmin, newmax;
 521         float *__restrict bv = node->bv;
 522         int j;
 523         axis_t axis_iter;
 524
 525         node_minmax_init(tree, node);
 526
 527         for (j = start; j < end; j++) {
 528                 float *__restrict node_bv = tree->nodes[j]->bv;
 529
 530                 /* for all Axes. */
 531                 for (axis_iter = tree->start_axis; axis_iter < tree->stop_axis; axis_iter++) {
 532                         newmin = node_bv[(2 * axis_iter)];
 533                         if ((newmin < bv[(2 * axis_iter)]))
 534                                 bv[(2 * axis_iter)] = newmin;
 535
 536                         newmax = node_bv[(2 * axis_iter) + 1];
 537                         if ((newmax > bv[(2 * axis_iter) + 1]))
 538                                 bv[(2 * axis_iter) + 1] = newmax;
 539                 }
 540         }
 541
 542 }
 543
 544 /**
 545  * only supports x,y,z axis in the moment
 546  * but we should use a plain and simple function here for speed sake */
 547 static char get_largest_axis(const float *bv)
 548 {
 549         float middle_point[3];
 550
 551         middle_point[0] = (bv[1]) - (bv[0]); /* x axis */
 552         middle_point[1] = (bv[3]) - (bv[2]); /* y axis */
 553         middle_point[2] = (bv[5]) - (bv[4]); /* z axis */
 554         if (middle_point[0] > middle_point[1]) {
 555                 if (middle_point[0] > middle_point[2])
 556                         return 1;  /* max x axis */
 557                 else
 558                         return 5;  /* max z axis */
 559         }
 560         else {
 561                 if (middle_point[1] > middle_point[2])
 562                         return 3;  /* max y axis */
 563                 else
 564                         return 5;  /* max z axis */
 565         }
 566 }
 567
 568 /**
 569  * bottom-up update of bvh node BV
 570  * join the children on the parent BV */
 571 static void node_join(BVHTree *tree, BVHNode *node)
 572 {
 573         int i;
 574         axis_t axis_iter;
 575
 576         node_minmax_init(tree, node);
 577
 578         for (i = 0; i < tree->tree_type; i++) {
 579                 if (node->children[i]) {
 580                         for (axis_iter = tree->start_axis; axis_iter < tree->stop_axis; axis_iter++) {
 581                                 /* update minimum */
 582                                 if (node->children[i]->bv[(2 * axis_iter)] < node->bv[(2 * axis_iter)])
 583                                         node->bv[(2 * axis_iter)] = node->children[i]->bv[(2 * axis_iter)];
 584
 585                                 /* update maximum */
 586                                 if (node->children[i]->bv[(2 * axis_iter) + 1] > node->bv[(2 * axis_iter) + 1])
 587                                         node->bv[(2 * axis_iter) + 1] = node->children[i]->bv[(2 * axis_iter) + 1];
 588                         }
 589                 }
 590                 else
 591                         break;
 592         }
 593 }
 594
 595 #ifdef USE_PRINT_TREE
 596
 597 /**
 598  * Debug and information functions
 599  */
 600
 601 static void bvhtree_print_tree(BVHTree *tree, BVHNode *node, int depth)
 602 {
 603         int i;
 604         axis_t axis_iter;
 605
 606         for (i = 0; i < depth; i++) printf(" ");
 607         printf(" - %d (%ld): ", node->index, (long int)(node - tree->nodearray));
 608         for (axis_iter = (axis_t)(2 * tree->start_axis);
 609              axis_iter < (axis_t)(2 * tree->stop_axis);
 610              axis_iter++)
 611         {
 612                 printf("%.3f ", node->bv[axis_iter]);
 613         }
 614         printf("\n");
 615
 616         for (i = 0; i < tree->tree_type; i++)
 617                 if (node->children[i])
 618                         bvhtree_print_tree(tree, node->children[i], depth + 1);
 619 }
 620
 621 static void bvhtree_info(BVHTree *tree)
 622 {
 623         printf("BVHTree Info: tree_type = %d, axis = %d, epsilon = %f\n",
 624                tree->tree_type, tree->axis, tree->epsilon);
 625         printf("nodes = %d, branches = %d, leafs = %d\n",
 626                tree->totbranch + tree->totleaf,  tree->totbranch, tree->totleaf);
 627         printf("Memory per node = %ubytes\n",
 628                (uint)(sizeof(BVHNode) + sizeof(BVHNode *) * tree->tree_type + sizeof(float) * tree->axis));
 629         printf("BV memory = %ubytes\n",
 630                (uint)MEM_allocN_len(tree->nodebv));
 631
 632         printf("Total memory = %ubytes\n",
 633                (uint)(sizeof(BVHTree) +
 634                       MEM_allocN_len(tree->nodes) +
 635                       MEM_allocN_len(tree->nodearray) +
 636                       MEM_allocN_len(tree->nodechild) +
 637                       MEM_allocN_len(tree->nodebv)));
 638
 639         bvhtree_print_tree(tree, tree->nodes[tree->totleaf], 0);
 640 }
 641 #endif  /* USE_PRINT_TREE */
 642
 643 #ifdef USE_VERIFY_TREE
 644
 645 static void bvhtree_verify(BVHTree *tree)
 646 {
 647         int i, j, check = 0;
 648
 649         /* check the pointer list */
 650         for (i = 0; i < tree->totleaf; i++) {
 651                 if (tree->nodes[i]->parent == NULL) {
 652                         printf("Leaf has no parent: %d\n", i);
 653                 }
 654                 else {
 655                         for (j = 0; j < tree->tree_type; j++) {
 656                                 if (tree->nodes[i]->parent->children[j] == tree->nodes[i])
 657                                         check = 1;
 658                         }
 659                         if (!check) {
 660                                 printf("Parent child relationship doesn't match: %d\n", i);
 661                         }
 662                         check = 0;
 663                 }
 664         }
 665
 666         /* check the leaf list */
 667         for (i = 0; i < tree->totleaf; i++) {
 668                 if (tree->nodearray[i].parent == NULL) {
 669                         printf("Leaf has no parent: %d\n", i);
 670                 }
 671                 else {
 672                         for (j = 0; j < tree->tree_type; j++) {
 673                                 if (tree->nodearray[i].parent->children[j] == &tree->nodearray[i])
 674                                         check = 1;
 675                         }
 676                         if (!check) {
 677                                 printf("Parent child relationship doesn't match: %d\n", i);
 678                         }
 679                         check = 0;
 680                 }
 681         }
 682
 683         printf("branches: %d, leafs: %d, total: %d\n",
 684                tree->totbranch, tree->totleaf, tree->totbranch + tree->totleaf);
 685 }
 686 #endif  /* USE_VERIFY_TREE */
 687
 688 /* Helper data and structures to build a min-leaf generalized implicit tree
 689  * This code can be easily reduced
 690  * (basicly this is only method to calculate pow(k, n) in O(1).. and stuff like that) */
 691 typedef struct BVHBuildHelper {
 692         int tree_type;              /* */
 693         int totleafs;               /* */
 694
 695         int leafs_per_child[32];    /* Min number of leafs that are archievable from a node at depth N */
 696         int branches_on_level[32];  /* Number of nodes at depth N (tree_type^N) */
 697
 698         int remain_leafs;           /* Number of leafs that are placed on the level that is not 100% filled */
 699
 700 } BVHBuildHelper;
 701
 702 static void build_implicit_tree_helper(const BVHTree *tree, BVHBuildHelper *data)
 703 {
 704         int depth = 0;
 705         int remain;
 706         int nnodes;
 707
 708         data->totleafs = tree->totleaf;
 709         data->tree_type = tree->tree_type;
 710
 711         /* Calculate the smallest tree_type^n such that tree_type^n >= num_leafs */
 712         for (data->leafs_per_child[0] = 1;
 713              data->leafs_per_child[0] <  data->totleafs;
 714              data->leafs_per_child[0] *= data->tree_type)
 715         {
 716                 /* pass */
 717         }
 718
 719         data->branches_on_level[0] = 1;
 720
 721         for (depth = 1; (depth < 32) && data->leafs_per_child[depth - 1]; depth++) {
 722                 data->branches_on_level[depth] = data->branches_on_level[depth - 1] * data->tree_type;
 723                 data->leafs_per_child[depth] = data->leafs_per_child[depth - 1] / data->tree_type;
 724         }
 725
 726         remain = data->totleafs - data->leafs_per_child[1];
 727         nnodes = (remain + data->tree_type - 2) / (data->tree_type - 1);
 728         data->remain_leafs = remain + nnodes;
 729 }
 730
 731 // return the min index of all the leafs archivable with the given branch
 732 static int implicit_leafs_index(const BVHBuildHelper *data, const int depth, const int child_index)
 733 {
 734         int min_leaf_index = child_index * data->leafs_per_child[depth - 1];
 735         if (min_leaf_index <= data->remain_leafs)
 736                 return min_leaf_index;
 737         else if (data->leafs_per_child[depth])
 738                 return data->totleafs - (data->branches_on_level[depth - 1] - child_index) * data->leafs_per_child[depth];
 739         else
 740                 return data->remain_leafs;
 741 }
 742
 743 /**
 744  * Generalized implicit tree build
 745  *
 746  * An implicit tree is a tree where its structure is implied, thus there is no need to store child pointers or indexs.
 747  * Its possible to find the position of the child or the parent with simple maths (multiplication and adittion).
 748  * This type of tree is for example used on heaps.. where node N has its childs at indexs N*2 and N*2+1.
 749  *
 750  * Although in this case the tree type is general.. and not know until runtime.
 751  * tree_type stands for the maximum number of childs that a tree node can have.
 752  * All tree types >= 2 are supported.
 753  *
 754  * Advantages of the used trees include:
 755  *  - No need to store child/parent relations (they are implicit);
 756  *  - Any node child always has an index greater than the parent;
 757  *  - Brother nodes are sequential in memory;
 758  *
 759  *
 760  * Some math relations derived for general implicit trees:
 761  *
 762  *   K = tree_type, ( 2 <= K )
 763  *   ROOT = 1
 764  *   N child of node A = A * K + (2 - K) + N, (0 <= N < K)
 765  *
 766  * Util methods:
 767  *   TODO...
 768  *    (looping elements, knowing if its a leaf or not.. etc...)
 769  */
 770
 771 /* This functions returns the number of branches needed to have the requested number of leafs. */
 772 static int implicit_needed_branches(int tree_type, int leafs)
 773 {
 774         return max_ii(1, (leafs + tree_type - 3) / (tree_type - 1));
 775 }
 776
 777 /**
 778  * This function handles the problem of "sorting" the leafs (along the split_axis).
 779  *
 780  * It arranges the elements in the given partitions such that:
 781  *  - any element in partition N is less or equal to any element in partition N+1.
 782  *  - if all elements are different all partition will get the same subset of elements
 783  *    as if the array was sorted.
 784  *
 785  * partition P is described as the elements in the range ( nth[P], nth[P+1] ]
 786  *
 787  * TODO: This can be optimized a bit by doing a specialized nth_element instead of K nth_elements
 788  */
 789 static void split_leafs(BVHNode **leafs_array, const int nth[], const int partitions, const int split_axis)
 790 {
 791         int i;
 792         for (i = 0; i < partitions - 1; i++) {
 793                 if (nth[i] >= nth[partitions])
 794                         break;
 795
 796                 partition_nth_element(leafs_array, nth[i], nth[partitions], nth[i + 1], split_axis);
 797         }
 798 }
 799
 800 typedef struct BVHDivNodesData {
 801         const BVHTree *tree;
 802         BVHNode *branches_array;
 803         BVHNode **leafs_array;
 804
 805         int tree_type;
 806         int tree_offset;
 807
 808         const BVHBuildHelper *data;
 809
 810         int depth;
 811         int i;
 812         int first_of_next_level;
 813 } BVHDivNodesData;
 814
 815 static void non_recursive_bvh_div_nodes_task_cb(
 816         void *__restrict userdata,
 817         const int j,
 818         const ParallelRangeTLS *__restrict UNUSED(tls))
 819 {
 820         BVHDivNodesData *data = userdata;
 821
 822         int k;
 823         const int parent_level_index = j - data->i;
 824         BVHNode *parent = &data->branches_array[j];
 825         int nth_positions[MAX_TREETYPE + 1];
 826         char split_axis;
 827
 828         int parent_leafs_begin = implicit_leafs_index(data->data, data->depth, parent_level_index);
 829         int parent_leafs_end   = implicit_leafs_index(data->data, data->depth, parent_level_index + 1);
 830
 831         /* This calculates the bounding box of this branch
 832          * and chooses the largest axis as the axis to divide leafs */
 833         refit_kdop_hull(data->tree, parent, parent_leafs_begin, parent_leafs_end);
 834         split_axis = get_largest_axis(parent->bv);
 835
 836         /* Save split axis (this can be used on raytracing to speedup the query time) */
 837         parent->main_axis = split_axis / 2;
 838
 839         /* Split the childs along the split_axis, note: its not needed to sort the whole leafs array
 840          * Only to assure that the elements are partitioned on a way that each child takes the elements
 841          * it would take in case the whole array was sorted.
 842          * Split_leafs takes care of that "sort" problem. */
 843         nth_positions[0] = parent_leafs_begin;
 844         nth_positions[data->tree_type] = parent_leafs_end;
 845         for (k = 1; k < data->tree_type; k++) {
 846                 const int child_index = j * data->tree_type + data->tree_offset + k;
 847                 const int child_level_index = child_index - data->first_of_next_level; /* child level index */
 848                 nth_positions[k] = implicit_leafs_index(data->data, data->depth + 1, child_level_index);
 849         }
 850
 851         split_leafs(data->leafs_array, nth_positions, data->tree_type, split_axis);
 852
 853         /* Setup children and totnode counters
 854          * Not really needed but currently most of BVH code relies on having an explicit children structure */
 855         for (k = 0; k < data->tree_type; k++) {
 856                 const int child_index = j * data->tree_type + data->tree_offset + k;
 857                 const int child_level_index = child_index - data->first_of_next_level; /* child level index */
 858
 859                 const int child_leafs_begin = implicit_leafs_index(data->data, data->depth + 1, child_level_index);
 860                 const int child_leafs_end   = implicit_leafs_index(data->data, data->depth + 1, child_level_index + 1);
 861
 862                 if (child_leafs_end - child_leafs_begin > 1) {
 863                         parent->children[k] = &data->branches_array[child_index];
 864                         parent->children[k]->parent = parent;
 865                 }
 866                 else if (child_leafs_end - child_leafs_begin == 1) {
 867                         parent->children[k] = data->leafs_array[child_leafs_begin];
 868                         parent->children[k]->parent = parent;
 869                 }
 870                 else {
 871                         break;
 872                 }
 873         }
 874         parent->totnode = (char)k;
 875 }
 876
 877 /**
 878  * This functions builds an optimal implicit tree from the given leafs.
 879  * Where optimal stands for:
 880  *  - The resulting tree will have the smallest number of branches;
 881  *  - At most only one branch will have NULL childs;
 882  *  - All leafs will be stored at level N or N+1.
 883  *
 884  * This function creates an implicit tree on branches_array, the leafs are given on the leafs_array.
 885  *
 886  * The tree is built per depth levels. First branches at depth 1.. then branches at depth 2.. etc..
 887  * The reason is that we can build level N+1 from level N without any data dependencies.. thus it allows
 888  * to use multithread building.
 889  *
 890  * To archive this is necessary to find how much leafs are accessible from a certain branch, BVHBuildHelper
 891  * #implicit_needed_branches and #implicit_leafs_index are auxiliary functions to solve that "optimal-split".
 892  */
 893 static void non_recursive_bvh_div_nodes(
 894         const BVHTree *tree, BVHNode *branches_array, BVHNode **leafs_array, int num_leafs)
 895 {
 896         int i;
 897
 898         const int tree_type   = tree->tree_type;
 899         const int tree_offset = 2 - tree->tree_type; /* this value is 0 (on binary trees) and negative on the others */
 900         const int num_branches = implicit_needed_branches(tree_type, num_leafs);
 901
 902         BVHBuildHelper data;
 903         int depth;
 904
 905         {
 906                 /* set parent from root node to NULL */
 907                 BVHNode *root = &branches_array[1];
 908                 root->parent = NULL;
 909
 910                 /* Most of bvhtree code relies on 1-leaf trees having at least one branch
 911                  * We handle that special case here */
 912                 if (num_leafs == 1) {
 913                         refit_kdop_hull(tree, root, 0, num_leafs);
 914                         root->main_axis = get_largest_axis(root->bv) / 2;
 915                         root->totnode = 1;
 916                         root->children[0] = leafs_array[0];
 917                         root->children[0]->parent = root;
 918                         return;
 919                 }
 920         }
 921
 922         build_implicit_tree_helper(tree, &data);
 923
 924         BVHDivNodesData cb_data = {
 925                 .tree = tree, .branches_array = branches_array, .leafs_array = leafs_array,
 926                 .tree_type = tree_type, .tree_offset = tree_offset, .data = &data,
 927                 .first_of_next_level = 0, .depth = 0, .i = 0,
 928         };
 929
 930         /* Loop tree levels (log N) loops */
 931         for (i = 1, depth = 1; i <= num_branches; i = i * tree_type + tree_offset, depth++) {
 932                 const int first_of_next_level = i * tree_type + tree_offset;
 933                 const int i_stop = min_ii(first_of_next_level, num_branches + 1);  /* index of last branch on this level */
 934
 935                 /* Loop all branches on this level */
 936                 cb_data.first_of_next_level = first_of_next_level;
 937                 cb_data.i = i;
 938                 cb_data.depth = depth;
 939
 940                 if (true) {
 941                         ParallelRangeSettings settings;
 942                         BLI_parallel_range_settings_defaults(&settings);
 943                         settings.use_threading = (num_leafs > KDOPBVH_THREAD_LEAF_THRESHOLD);
 944                         BLI_task_parallel_range(
 945                                 i, i_stop,
 946                                 &cb_data,
 947                                 non_recursive_bvh_div_nodes_task_cb,
 948                                 &settings);
 949                 }
 950                 else {
 951                         /* Less hassle for debugging. */
 952                         ParallelRangeTLS tls = {0};
 953                         for (int i_task = i; i_task < i_stop; i_task++) {
 954                                 non_recursive_bvh_div_nodes_task_cb(&cb_data, i_task, &tls);
 955                         }
 956                 }
 957         }
 958 }
 959
 960 /** \} */
 961
 962
 963 /* -------------------------------------------------------------------- */
 964
 965 /** \name BLI_bvhtree API
 966  * \{ */
 967
 968 /**
 969  * \note many callers don't check for ``NULL`` return.
 970  */
 971 BVHTree *BLI_bvhtree_new(int maxsize, float epsilon, char tree_type, char axis)
 972 {
 973         BVHTree *tree;
 974         int numnodes, i;
 975
 976         BLI_assert(tree_type >= 2 && tree_type <= MAX_TREETYPE);
 977
 978         tree = MEM_callocN(sizeof(BVHTree), "BVHTree");
 979
 980         /* tree epsilon must be >= FLT_EPSILON
 981          * so that tangent rays can still hit a bounding volume..
 982          * this bug would show up when casting a ray aligned with a kdop-axis and with an edge of 2 faces */
 983         epsilon = max_ff(FLT_EPSILON, epsilon);
 984
 985         if (tree) {
 986                 tree->epsilon = epsilon;
 987                 tree->tree_type = tree_type;
 988                 tree->axis = axis;
 989
 990                 if (axis == 26) {
 991                         tree->start_axis = 0;
 992                         tree->stop_axis = 13;
 993                 }
 994                 else if (axis == 18) {
 995                         tree->start_axis = 7;
 996                         tree->stop_axis = 13;
 997                 }
 998                 else if (axis == 14) {
 999                         tree->start_axis = 0;
1000                         tree->stop_axis = 7;
1001                 }
1002                 else if (axis == 8) { /* AABB */
1003                         tree->start_axis = 0;
1004                         tree->stop_axis = 4;
1005                 }
1006                 else if (axis == 6) { /* OBB */
1007                         tree->start_axis = 0;
1008                         tree->stop_axis = 3;
1009                 }
1010                 else {
1011                         /* should never happen! */
1012                         BLI_assert(0);
1013
1014                         goto fail;
1015                 }
1016
1017
1018                 /* Allocate arrays */
1019                 numnodes = maxsize + implicit_needed_branches(tree_type, maxsize) + tree_type;
1020
1021                 tree->nodes = MEM_callocN(sizeof(BVHNode *) * (size_t)numnodes, "BVHNodes");
1022                 tree->nodebv = MEM_callocN(sizeof(float) * (size_t)(axis * numnodes), "BVHNodeBV");
1023                 tree->nodechild = MEM_callocN(sizeof(BVHNode *) * (size_t)(tree_type * numnodes), "BVHNodeBV");
1024                 tree->nodearray = MEM_callocN(sizeof(BVHNode) * (size_t)numnodes, "BVHNodeArray");
1025
1026                 if (UNLIKELY((!tree->nodes) ||
1027                              (!tree->nodebv) ||
1028                              (!tree->nodechild) ||
1029                              (!tree->nodearray)))
1030                 {
1031                         goto fail;
1032                 }
1033
1034                 /* link the dynamic bv and child links */
1035                 for (i = 0; i < numnodes; i++) {
1036                         tree->nodearray[i].bv = &tree->nodebv[i * axis];
1037                         tree->nodearray[i].children = &tree->nodechild[i * tree_type];
1038                 }
1039
1040         }
1041         return tree;
1042
1043
1044 fail:
1045         MEM_SAFE_FREE(tree->nodes);
1046         MEM_SAFE_FREE(tree->nodebv);
1047         MEM_SAFE_FREE(tree->nodechild);
1048         MEM_SAFE_FREE(tree->nodearray);
1049
1050         MEM_freeN(tree);
1051
1052         return NULL;
1053 }
1054
1055 void BLI_bvhtree_free(BVHTree *tree)
1056 {
1057         if (tree) {
1058                 MEM_freeN(tree->nodes);
1059                 MEM_freeN(tree->nodearray);
1060                 MEM_freeN(tree->nodebv);
1061                 MEM_freeN(tree->nodechild);
1062                 MEM_freeN(tree);
1063         }
1064 }
1065
1066 void BLI_bvhtree_balance(BVHTree *tree)
1067 {
1068         BVHNode **leafs_array    = tree->nodes;
1069
1070         /* This function should only be called once
1071          * (some big bug goes here if its being called more than once per tree) */
1072         BLI_assert(tree->totbranch == 0);
1073
1074         /* Build the implicit tree */
1075         non_recursive_bvh_div_nodes(tree, tree->nodearray + (tree->totleaf - 1), leafs_array, tree->totleaf);
1076
1077         /* current code expects the branches to be linked to the nodes array
1078          * we perform that linkage here */
1079         tree->totbranch = implicit_needed_branches(tree->tree_type, tree->totleaf);
1080         for (int i = 0; i < tree->totbranch; i++) {
1081                 tree->nodes[tree->totleaf + i] = &tree->nodearray[tree->totleaf + i];
1082         }
1083
1084 #ifdef USE_SKIP_LINKS
1085         build_skip_links(tree, tree->nodes[tree->totleaf], NULL, NULL);
1086 #endif
1087
1088 #ifdef USE_VERIFY_TREE
1089         bvhtree_verify(tree);
1090 #endif
1091
1092 #ifdef USE_PRINT_TREE
1093         bvhtree_info(tree);
1094 #endif
1095 }
1096
1097 void BLI_bvhtree_insert(BVHTree *tree, int index, const float co[3], int numpoints)
1098 {
1099         axis_t axis_iter;
1100         BVHNode *node = NULL;
1101
1102         /* insert should only possible as long as tree->totbranch is 0 */
1103         BLI_assert(tree->totbranch <= 0);
1104         BLI_assert((size_t)tree->totleaf < MEM_allocN_len(tree->nodes) / sizeof(*(tree->nodes)));
1105
1106         node = tree->nodes[tree->totleaf] = &(tree->nodearray[tree->totleaf]);
1107         tree->totleaf++;
1108
1109         create_kdop_hull(tree, node, co, numpoints, 0);
1110         node->index = index;
1111
1112         /* inflate the bv with some epsilon */
1113         for (axis_iter = tree->start_axis; axis_iter < tree->stop_axis; axis_iter++) {
1114                 node->bv[(2 * axis_iter)] -= tree->epsilon; /* minimum */
1115                 node->bv[(2 * axis_iter) + 1] += tree->epsilon; /* maximum */
1116         }
1117 }
1118
1119
1120 /* call before BLI_bvhtree_update_tree() */
1121 bool BLI_bvhtree_update_node(BVHTree *tree, int index, const float co[3], const float co_moving[3], int numpoints)
1122 {
1123         BVHNode *node = NULL;
1124         axis_t axis_iter;
1125
1126         /* check if index exists */
1127         if (index > tree->totleaf)
1128                 return false;
1129
1130         node = tree->nodearray + index;
1131
1132         create_kdop_hull(tree, node, co, numpoints, 0);
1133
1134         if (co_moving)
1135                 create_kdop_hull(tree, node, co_moving, numpoints, 1);
1136
1137         /* inflate the bv with some epsilon */
1138         for (axis_iter = tree->start_axis; axis_iter < tree->stop_axis; axis_iter++) {
1139                 node->bv[(2 * axis_iter)]     -= tree->epsilon; /* minimum */
1140                 node->bv[(2 * axis_iter) + 1] += tree->epsilon; /* maximum */
1141         }
1142
1143         return true;
1144 }
1145
1146 /* call BLI_bvhtree_update_node() first for every node/point/triangle */
1147 void BLI_bvhtree_update_tree(BVHTree *tree)
1148 {
1149         /* Update bottom=>top
1150          * TRICKY: the way we build the tree all the childs have an index greater than the parent
1151          * This allows us todo a bottom up update by starting on the bigger numbered branch */
1152
1153         BVHNode **root  = tree->nodes + tree->totleaf;
1154         BVHNode **index = tree->nodes + tree->totleaf + tree->totbranch - 1;
1155
1156         for (; index >= root; index--)
1157                 node_join(tree, *index);
1158 }
1159 /**
1160  * Number of times #BLI_bvhtree_insert has been called.
1161  * mainly useful for asserts functions to check we added the correct number.
1162  */
1163 int BLI_bvhtree_get_len(const BVHTree *tree)
1164 {
1165         return tree->totleaf;
1166 }
1167
1168 /**
1169  * Maximum number of children that a node can have.
1170  */
1171 int BLI_bvhtree_get_tree_type(const BVHTree *tree)
1172 {
1173         return tree->tree_type;
1174 }
1175
1176 float BLI_bvhtree_get_epsilon(const BVHTree *tree)
1177 {
1178         return tree->epsilon;
1179 }
1180
1181 /** \} */
1182
1183
1184 /* -------------------------------------------------------------------- */
1185
1186 /** \name BLI_bvhtree_overlap
1187  * \{ */
1188
1189 /**
1190  * overlap - is it possible for 2 bv's to collide ?
1191  */
1192 static bool tree_overlap_test(const BVHNode *node1, const BVHNode *node2, axis_t start_axis, axis_t stop_axis)
1193 {
1194         const float *bv1     = node1->bv + (start_axis << 1);
1195         const float *bv2     = node2->bv + (start_axis << 1);
1196         const float *bv1_end = node1->bv + (stop_axis  << 1);
1197
1198         /* test all axis if min + max overlap */
1199         for (; bv1 != bv1_end; bv1 += 2, bv2 += 2) {
1200                 if ((bv1[0] > bv2[1]) || (bv2[0] > bv1[1])) {
1201                         return 0;
1202                 }
1203         }
1204
1205         return 1;
1206 }
1207
1208 static void tree_overlap_traverse(
1209         BVHOverlapData_Thread *data_thread,
1210         const BVHNode *node1, const BVHNode *node2)
1211 {
1212         BVHOverlapData_Shared *data = data_thread->shared;
1213         int j;
1214
1215         if (tree_overlap_test(node1, node2, data->start_axis, data->stop_axis)) {
1216                 /* check if node1 is a leaf */
1217                 if (!node1->totnode) {
1218                         /* check if node2 is a leaf */
1219                         if (!node2->totnode) {
1220                                 BVHTreeOverlap *overlap;
1221
1222                                 if (UNLIKELY(node1 == node2)) {
1223                                         return;
1224                                 }
1225
1226                                 /* both leafs, insert overlap! */
1227                                 overlap = BLI_stack_push_r(data_thread->overlap);
1228                                 overlap->indexA = node1->index;
1229                                 overlap->indexB = node2->index;
1230                         }
1231                         else {
1232                                 for (j = 0; j < data->tree2->tree_type; j++) {
1233                                         if (node2->children[j]) {
1234                                                 tree_overlap_traverse(data_thread, node1, node2->children[j]);
1235                                         }
1236                                 }
1237                         }
1238                 }
1239                 else {
1240                         for (j = 0; j < data->tree2->tree_type; j++) {
1241                                 if (node1->children[j]) {
1242                                         tree_overlap_traverse(data_thread, node1->children[j], node2);
1243                                 }
1244                         }
1245                 }
1246         }
1247 }
1248
1249 /**
1250  * a version of #tree_overlap_traverse that runs a callback to check if the nodes really intersect.
1251  */
1252 static void tree_overlap_traverse_cb(
1253         BVHOverlapData_Thread *data_thread,
1254         const BVHNode *node1, const BVHNode *node2)
1255 {
1256         BVHOverlapData_Shared *data = data_thread->shared;
1257         int j;
1258
1259         if (tree_overlap_test(node1, node2, data->start_axis, data->stop_axis)) {
1260                 /* check if node1 is a leaf */
1261                 if (!node1->totnode) {
1262                         /* check if node2 is a leaf */
1263                         if (!node2->totnode) {
1264                                 BVHTreeOverlap *overlap;
1265
1266                                 if (UNLIKELY(node1 == node2)) {
1267                                         return;
1268                                 }
1269
1270                                 /* only difference to tree_overlap_traverse! */
1271                                 if (data->callback(data->userdata, node1->index, node2->index, data_thread->thread)) {
1272                                         /* both leafs, insert overlap! */
1273                                         overlap = BLI_stack_push_r(data_thread->overlap);
1274                                         overlap->indexA = node1->index;
1275                                         overlap->indexB = node2->index;
1276                                 }
1277                         }
1278                         else {
1279                                 for (j = 0; j < data->tree2->tree_type; j++) {
1280                                         if (node2->children[j]) {
1281                                                 tree_overlap_traverse_cb(data_thread, node1, node2->children[j]);
1282                                         }
1283                                 }
1284                         }
1285                 }
1286                 else {
1287                         for (j = 0; j < data->tree2->tree_type; j++) {
1288                                 if (node1->children[j]) {
1289                                         tree_overlap_traverse_cb(data_thread, node1->children[j], node2);
1290                                 }
1291                         }
1292                 }
1293         }
1294 }
1295
1296 /**
1297  * Use to check the total number of threads #BLI_bvhtree_overlap will use.
1298  *
1299  * \warning Must be the first tree passed to #BLI_bvhtree_overlap!
1300  */
1301 int BLI_bvhtree_overlap_thread_num(const BVHTree *tree)
1302 {
1303         return (int)MIN2(tree->tree_type, tree->nodes[tree->totleaf]->totnode);
1304 }
1305
1306 static void bvhtree_overlap_task_cb(
1307         void *__restrict userdata,
1308         const int j,
1309         const ParallelRangeTLS *__restrict UNUSED(tls))
1310 {
1311         BVHOverlapData_Thread *data = &((BVHOverlapData_Thread *)userdata)[j];
1312         BVHOverlapData_Shared *data_shared = data->shared;
1313
1314         if (data_shared->callback) {
1315                 tree_overlap_traverse_cb(
1316                             data, data_shared->tree1->nodes[data_shared->tree1->totleaf]->children[j],
1317                             data_shared->tree2->nodes[data_shared->tree2->totleaf]);
1318         }
1319         else {
1320                 tree_overlap_traverse(
1321                             data, data_shared->tree1->nodes[data_shared->tree1->totleaf]->children[j],
1322                             data_shared->tree2->nodes[data_shared->tree2->totleaf]);
1323         }
1324 }
1325
1326 BVHTreeOverlap *BLI_bvhtree_overlap(
1327         const BVHTree *tree1, const BVHTree *tree2, uint *r_overlap_tot,
1328         /* optional callback to test the overlap before adding (must be thread-safe!) */
1329         BVHTree_OverlapCallback callback, void *userdata)
1330 {
1331         const int thread_num = BLI_bvhtree_overlap_thread_num(tree1);
1332         int j;
1333         size_t total = 0;
1334         BVHTreeOverlap *overlap = NULL, *to = NULL;
1335         BVHOverlapData_Shared data_shared;
1336         BVHOverlapData_Thread *data = BLI_array_alloca(data, (size_t)thread_num);
1337         axis_t start_axis, stop_axis;
1338
1339         /* check for compatibility of both trees (can't compare 14-DOP with 18-DOP) */
1340         if (UNLIKELY((tree1->axis != tree2->axis) &&
1341                      (tree1->axis == 14 || tree2->axis == 14) &&
1342                      (tree1->axis == 18 || tree2->axis == 18)))
1343         {
1344                 BLI_assert(0);
1345                 return NULL;
1346         }
1347
1348         start_axis = min_axis(tree1->start_axis, tree2->start_axis);
1349         stop_axis  = min_axis(tree1->stop_axis,  tree2->stop_axis);
1350
1351         /* fast check root nodes for collision before doing big splitting + traversal */
1352         if (!tree_overlap_test(tree1->nodes[tree1->totleaf], tree2->nodes[tree2->totleaf], start_axis, stop_axis)) {
1353                 return NULL;
1354         }
1355
1356         data_shared.tree1 = tree1;
1357         data_shared.tree2 = tree2;
1358         data_shared.start_axis = start_axis;
1359         data_shared.stop_axis = stop_axis;
1360
1361         /* can be NULL */
1362         data_shared.callback = callback;
1363         data_shared.userdata = userdata;
1364
1365         for (j = 0; j < thread_num; j++) {
1366                 /* init BVHOverlapData_Thread */
1367                 data[j].shared = &data_shared;
1368                 data[j].overlap = BLI_stack_new(sizeof(BVHTreeOverlap), __func__);
1369
1370                 /* for callback */
1371                 data[j].thread = j;
1372         }
1373
1374         ParallelRangeSettings settings;
1375         BLI_parallel_range_settings_defaults(&settings);
1376         settings.use_threading = (tree1->totleaf > KDOPBVH_THREAD_LEAF_THRESHOLD);
1377         BLI_task_parallel_range(
1378                     0, thread_num,
1379                     data,
1380                     bvhtree_overlap_task_cb,
1381                     &settings);
1382
1383         for (j = 0; j < thread_num; j++)
1384                 total += BLI_stack_count(data[j].overlap);
1385
1386         to = overlap = MEM_mallocN(sizeof(BVHTreeOverlap) * total, "BVHTreeOverlap");
1387
1388         for (j = 0; j < thread_num; j++) {
1389                 uint count = (uint)BLI_stack_count(data[j].overlap);
1390                 BLI_stack_pop_n(data[j].overlap, to, count);
1391                 BLI_stack_free(data[j].overlap);
1392                 to += count;
1393         }
1394
1395         *r_overlap_tot = (uint)total;
1396         return overlap;
1397 }
1398
1399 /** \} */
1400
1401
1402 /* -------------------------------------------------------------------- */
1403
1404 /** \name BLI_bvhtree_find_nearest
1405  * \{ */
1406
1407 /* Determines the nearest point of the given node BV. Returns the squared distance to that point. */
1408 static float calc_nearest_point_squared(const float proj[3], BVHNode *node, float nearest[3])
1409 {
1410         int i;
1411         const float *bv = node->bv;
1412
1413         /* nearest on AABB hull */
1414         for (i = 0; i != 3; i++, bv += 2) {
1415                 if (bv[0] > proj[i])
1416                         nearest[i] = bv[0];
1417                 else if (bv[1] < proj[i])
1418                         nearest[i] = bv[1];
1419                 else
1420                         nearest[i] = proj[i];
1421         }
1422
1423 #if 0
1424         /* nearest on a general hull */
1425         copy_v3_v3(nearest, data->co);
1426         for (i = data->tree->start_axis; i != data->tree->stop_axis; i++, bv += 2) {
1427                 float proj = dot_v3v3(nearest, bvhtree_kdop_axes[i]);
1428                 float dl = bv[0] - proj;
1429                 float du = bv[1] - proj;
1430
1431                 if (dl > 0) {
1432                         madd_v3_v3fl(nearest, bvhtree_kdop_axes[i], dl);
1433                 }
1434                 else if (du < 0) {
1435                         madd_v3_v3fl(nearest, bvhtree_kdop_axes[i], du);
1436                 }
1437         }
1438 #endif
1439
1440         return len_squared_v3v3(proj, nearest);
1441 }
1442
1443 /* TODO: use a priority queue to reduce the number of nodes looked on */
1444 static void dfs_find_nearest_dfs(BVHNearestData *data, BVHNode *node)
1445 {
1446         if (node->totnode == 0) {
1447                 if (data->callback)
1448                         data->callback(data->userdata, node->index, data->co, &data->nearest);
1449                 else {
1450                         data->nearest.index = node->index;
1451                         data->nearest.dist_sq = calc_nearest_point_squared(data->proj, node, data->nearest.co);
1452                 }
1453         }
1454         else {
1455                 /* Better heuristic to pick the closest node to dive on */
1456                 int i;
1457                 float nearest[3];
1458
1459                 if (data->proj[node->main_axis] <= node->children[0]->bv[node->main_axis * 2 + 1]) {
1460
1461                         for (i = 0; i != node->totnode; i++) {
1462                                 if (calc_nearest_point_squared(data->proj, node->children[i], nearest) >= data->nearest.dist_sq)
1463                                         continue;
1464                                 dfs_find_nearest_dfs(data, node->children[i]);
1465                         }
1466                 }
1467                 else {
1468                         for (i = node->totnode - 1; i >= 0; i--) {
1469                                 if (calc_nearest_point_squared(data->proj, node->children[i], nearest) >= data->nearest.dist_sq)
1470                                         continue;
1471                                 dfs_find_nearest_dfs(data, node->children[i]);
1472                         }
1473                 }
1474         }
1475 }
1476
1477 static void dfs_find_nearest_begin(BVHNearestData *data, BVHNode *node)
1478 {
1479         float nearest[3], dist_sq;
1480         dist_sq = calc_nearest_point_squared(data->proj, node, nearest);
1481         if (dist_sq >= data->nearest.dist_sq) {
1482                 return;
1483         }
1484         dfs_find_nearest_dfs(data, node);
1485 }
1486
1487
1488 #if 0
1489
1490 typedef struct NodeDistance {
1491         BVHNode *node;
1492         float dist;
1493
1494 } NodeDistance;
1495
1496 #define DEFAULT_FIND_NEAREST_HEAP_SIZE 1024
1497
1498 #define NodeDistance_priority(a, b) ((a).dist < (b).dist)
1499
1500 static void NodeDistance_push_heap(NodeDistance *heap, int heap_size)
1501 PUSH_HEAP_BODY(NodeDistance, NodeDistance_priority, heap, heap_size)
1502
1503 static void NodeDistance_pop_heap(NodeDistance *heap, int heap_size)
1504 POP_HEAP_BODY(NodeDistance, NodeDistance_priority, heap, heap_size)
1505
1506 /* NN function that uses an heap.. this functions leads to an optimal number of min-distance
1507  * but for normal tri-faces and BV 6-dop.. a simple dfs with local heuristics (as implemented
1508  * in source/blender/blenkernel/intern/shrinkwrap.c) works faster.
1509  *
1510  * It may make sense to use this function if the callback queries are very slow.. or if its impossible
1511  * to get a nice heuristic
1512  *
1513  * this function uses "malloc/free" instead of the MEM_* because it intends to be thread safe */
1514 static void bfs_find_nearest(BVHNearestData *data, BVHNode *node)
1515 {
1516         int i;
1517         NodeDistance default_heap[DEFAULT_FIND_NEAREST_HEAP_SIZE];
1518         NodeDistance *heap = default_heap, current;
1519         int heap_size = 0, max_heap_size = sizeof(default_heap) / sizeof(default_heap[0]);
1520         float nearest[3];
1521
1522         int callbacks = 0, push_heaps = 0;
1523
1524         if (node->totnode == 0) {
1525                 dfs_find_nearest_dfs(data, node);
1526                 return;
1527         }
1528
1529         current.node = node;
1530         current.dist = calc_nearest_point(data->proj, node, nearest);
1531
1532         while (current.dist < data->nearest.dist) {
1533 //              printf("%f : %f\n", current.dist, data->nearest.dist);
1534                 for (i = 0; i < current.node->totnode; i++) {
1535                         BVHNode *child = current.node->children[i];
1536                         if (child->totnode == 0) {
1537                                 callbacks++;
1538                                 dfs_find_nearest_dfs(data, child);
1539                         }
1540                         else {
1541                                 /* adjust heap size */
1542                                 if ((heap_size >= max_heap_size) &&
1543                                     ADJUST_MEMORY(default_heap, (void **)&heap,
1544                                                   heap_size + 1, &max_heap_size, sizeof(heap[0])) == false)
1545                                 {
1546                                         printf("WARNING: bvh_find_nearest got out of memory\n");
1547
1548                                         if (heap != default_heap)
1549                                                 free(heap);
1550
1551                                         return;
1552                                 }
1553
1554                                 heap[heap_size].node = current.node->children[i];
1555                                 heap[heap_size].dist = calc_nearest_point(data->proj, current.node->children[i], nearest);
1556
1557                                 if (heap[heap_size].dist >= data->nearest.dist) continue;
1558                                 heap_size++;
1559
1560                                 NodeDistance_push_heap(heap, heap_size);
1561                                 //                      PUSH_HEAP_BODY(NodeDistance, NodeDistance_priority, heap, heap_size);
1562                                 push_heaps++;
1563                         }
1564                 }
1565
1566                 if (heap_size == 0) break;
1567
1568                 current = heap[0];
1569                 NodeDistance_pop_heap(heap, heap_size);
1570 //              POP_HEAP_BODY(NodeDistance, NodeDistance_priority, heap, heap_size);
1571                 heap_size--;
1572         }
1573
1574 //      printf("hsize=%d, callbacks=%d, pushs=%d\n", heap_size, callbacks, push_heaps);
1575
1576         if (heap != default_heap)
1577                 free(heap);
1578 }
1579 #endif
1580
1581
1582 int BLI_bvhtree_find_nearest(
1583         BVHTree *tree, const float co[3], BVHTreeNearest *nearest,
1584         BVHTree_NearestPointCallback callback, void *userdata)
1585 {
1586         axis_t axis_iter;
1587
1588         BVHNearestData data;
1589         BVHNode *root = tree->nodes[tree->totleaf];
1590
1591         /* init data to search */
1592         data.tree = tree;
1593         data.co = co;
1594
1595         data.callback = callback;
1596         data.userdata = userdata;
1597
1598         for (axis_iter = data.tree->start_axis; axis_iter != data.tree->stop_axis; axis_iter++) {
1599                 data.proj[axis_iter] = dot_v3v3(data.co, bvhtree_kdop_axes[axis_iter]);
1600         }
1601
1602         if (nearest) {
1603                 memcpy(&data.nearest, nearest, sizeof(*nearest));
1604         }
1605         else {
1606                 data.nearest.index = -1;
1607                 data.nearest.dist_sq = FLT_MAX;
1608         }
1609
1610         /* dfs search */
1611         if (root)
1612                 dfs_find_nearest_begin(&data, root);
1613
1614         /* copy back results */
1615         if (nearest) {
1616                 memcpy(nearest, &data.nearest, sizeof(*nearest));
1617         }
1618
1619         return data.nearest.index;
1620 }
1621
1622 /** \} */
1623
1624
1625 /* -------------------------------------------------------------------- */
1626
1627 /** \name BLI_bvhtree_ray_cast
1628  *
1629  * raycast is done by performing a DFS on the BVHTree and saving the closest hit.
1630  *
1631  * \{ */
1632
1633
1634 /* Determines the distance that the ray must travel to hit the bounding volume of the given node */
1635 static float ray_nearest_hit(const BVHRayCastData *data, const float bv[6])
1636 {
1637         int i;
1638
1639         float low = 0, upper = data->hit.dist;
1640
1641         for (i = 0; i != 3; i++, bv += 2) {
1642                 if (data->ray_dot_axis[i] == 0.0f) {
1643                         /* axis aligned ray */
1644                         if (data->ray.origin[i] < bv[0] - data->ray.radius ||
1645                             data->ray.origin[i] > bv[1] + data->ray.radius)
1646                         {
1647                                 return FLT_MAX;
1648                         }
1649                 }
1650                 else {
1651                         float ll = (bv[0] - data->ray.radius - data->ray.origin[i]) / data->ray_dot_axis[i];
1652                         float lu = (bv[1] + data->ray.radius - data->ray.origin[i]) / data->ray_dot_axis[i];
1653
1654                         if (data->ray_dot_axis[i] > 0.0f) {
1655                                 if (ll > low) low = ll;
1656                                 if (lu < upper) upper = lu;
1657                         }
1658                         else {
1659                                 if (lu > low) low = lu;
1660                                 if (ll < upper) upper = ll;
1661                         }
1662
1663                         if (low > upper) return FLT_MAX;
1664                 }
1665         }
1666         return low;
1667 }
1668
1669 /**
1670  * Determines the distance that the ray must travel to hit the bounding volume of the given node
1671  * Based on Tactical Optimization of Ray/Box Intersection, by Graham Fyffe
1672  * [http://tog.acm.org/resources/RTNews/html/rtnv21n1.html#art9]
1673  *
1674  * TODO this doesn't take data->ray.radius into consideration */
1675 static float fast_ray_nearest_hit(const BVHRayCastData *data, const BVHNode *node)
1676 {
1677         const float *bv = node->bv;
1678
1679         float t1x = (bv[data->index[0]] - data->ray.origin[0]) * data->idot_axis[0];
1680         float t2x = (bv[data->index[1]] - data->ray.origin[0]) * data->idot_axis[0];
1681         float t1y = (bv[data->index[2]] - data->ray.origin[1]) * data->idot_axis[1];
1682         float t2y = (bv[data->index[3]] - data->ray.origin[1]) * data->idot_axis[1];
1683         float t1z = (bv[data->index[4]] - data->ray.origin[2]) * data->idot_axis[2];
1684         float t2z = (bv[data->index[5]] - data->ray.origin[2]) * data->idot_axis[2];
1685
1686         if ((t1x > t2y || t2x < t1y || t1x > t2z || t2x < t1z || t1y > t2z || t2y < t1z) ||
1687             (t2x < 0.0f || t2y < 0.0f || t2z < 0.0f) ||
1688             (t1x > data->hit.dist || t1y > data->hit.dist || t1z > data->hit.dist))
1689         {
1690                 return FLT_MAX;
1691         }
1692         else {
1693                 return max_fff(t1x, t1y, t1z);
1694         }
1695 }
1696
1697 static void dfs_raycast(BVHRayCastData *data, BVHNode *node)
1698 {
1699         int i;
1700
1701         /* ray-bv is really fast.. and simple tests revealed its worth to test it
1702          * before calling the ray-primitive functions */
1703         /* XXX: temporary solution for particles until fast_ray_nearest_hit supports ray.radius */
1704         float dist = (data->ray.radius == 0.0f) ? fast_ray_nearest_hit(data, node) : ray_nearest_hit(data, node->bv);
1705         if (dist >= data->hit.dist) {
1706                 return;
1707         }
1708
1709         if (node->totnode == 0) {
1710                 if (data->callback) {
1711                         data->callback(data->userdata, node->index, &data->ray, &data->hit);
1712                 }
1713                 else {
1714                         data->hit.index = node->index;
1715                         data->hit.dist  = dist;
1716                         madd_v3_v3v3fl(data->hit.co, data->ray.origin, data->ray.direction, dist);
1717                 }
1718         }
1719         else {
1720                 /* pick loop direction to dive into the tree (based on ray direction and split axis) */
1721                 if (data->ray_dot_axis[node->main_axis] > 0.0f) {
1722                         for (i = 0; i != node->totnode; i++) {
1723                                 dfs_raycast(data, node->children[i]);
1724                         }
1725                 }
1726                 else {
1727                         for (i = node->totnode - 1; i >= 0; i--) {
1728                                 dfs_raycast(data, node->children[i]);
1729                         }
1730                 }
1731         }
1732 }
1733
1734 /**
1735  * A version of #dfs_raycast with minor changes to reset the index & dist each ray cast.
1736  */
1737 static void dfs_raycast_all(BVHRayCastData *data, BVHNode *node)
1738 {
1739         int i;
1740
1741         /* ray-bv is really fast.. and simple tests revealed its worth to test it
1742          * before calling the ray-primitive functions */
1743         /* XXX: temporary solution for particles until fast_ray_nearest_hit supports ray.radius */
1744         float dist = (data->ray.radius == 0.0f) ? fast_ray_nearest_hit(data, node) : ray_nearest_hit(data, node->bv);
1745         if (dist >= data->hit.dist) {
1746                 return;
1747         }
1748
1749         if (node->totnode == 0) {
1750                 /* no need to check for 'data->callback' (using 'all' only makes sense with a callback). */
1751                 dist = data->hit.dist;
1752                 data->callback(data->userdata, node->index, &data->ray, &data->hit);
1753                 data->hit.index = -1;
1754                 data->hit.dist = dist;
1755         }
1756         else {
1757                 /* pick loop direction to dive into the tree (based on ray direction and split axis) */
1758                 if (data->ray_dot_axis[node->main_axis] > 0.0f) {
1759                         for (i = 0; i != node->totnode; i++) {
1760                                 dfs_raycast_all(data, node->children[i]);
1761                         }
1762                 }
1763                 else {
1764                         for (i = node->totnode - 1; i >= 0; i--) {
1765                                 dfs_raycast_all(data, node->children[i]);
1766                         }
1767                 }
1768         }
1769 }
1770
1771 #if 0
1772 static void iterative_raycast(BVHRayCastData *data, BVHNode *node)
1773 {
1774         while (node) {
1775                 float dist = fast_ray_nearest_hit(data, node);
1776                 if (dist >= data->hit.dist) {
1777                         node = node->skip[1];
1778                         continue;
1779                 }
1780
1781                 if (node->totnode == 0) {
1782                         if (data->callback) {
1783                                 data->callback(data->userdata, node->index, &data->ray, &data->hit);
1784                         }
1785                         else {
1786                                 data->hit.index = node->index;
1787                                 data->hit.dist  = dist;
1788                                 madd_v3_v3v3fl(data->hit.co, data->ray.origin, data->ray.direction, dist);
1789                         }
1790
1791                         node = node->skip[1];
1792                 }
1793                 else {
1794                         node = node->children[0];
1795                 }
1796         }
1797 }
1798 #endif
1799
1800 static void bvhtree_ray_cast_data_precalc(BVHRayCastData *data, int flag)
1801 {
1802         int i;
1803
1804         for (i = 0; i < 3; i++) {
1805                 data->ray_dot_axis[i] = dot_v3v3(data->ray.direction, bvhtree_kdop_axes[i]);
1806                 data->idot_axis[i] = 1.0f / data->ray_dot_axis[i];
1807
1808                 if (fabsf(data->ray_dot_axis[i]) < FLT_EPSILON) {
1809                         data->ray_dot_axis[i] = 0.0;
1810                 }
1811                 data->index[2 * i] = data->idot_axis[i] < 0.0f ? 1 : 0;
1812                 data->index[2 * i + 1] = 1 - data->index[2 * i];
1813                 data->index[2 * i]   += 2 * i;
1814                 data->index[2 * i + 1] += 2 * i;
1815         }
1816
1817 #ifdef USE_KDOPBVH_WATERTIGHT
1818         if (flag & BVH_RAYCAST_WATERTIGHT) {
1819                 isect_ray_tri_watertight_v3_precalc(&data->isect_precalc, data->ray.direction);
1820                 data->ray.isect_precalc = &data->isect_precalc;
1821         }
1822         else {
1823                 data->ray.isect_precalc = NULL;
1824         }
1825 #else
1826         UNUSED_VARS(flag);
1827 #endif
1828 }
1829
1830 int BLI_bvhtree_ray_cast_ex(
1831         BVHTree *tree, const float co[3], const float dir[3], float radius, BVHTreeRayHit *hit,
1832         BVHTree_RayCastCallback callback, void *userdata,
1833         int flag)
1834 {
1835         BVHRayCastData data;
1836         BVHNode *root = tree->nodes[tree->totleaf];
1837
1838         BLI_ASSERT_UNIT_V3(dir);
1839
1840         data.tree = tree;
1841
1842         data.callback = callback;
1843         data.userdata = userdata;
1844
1845         copy_v3_v3(data.ray.origin,    co);
1846         copy_v3_v3(data.ray.direction, dir);
1847         data.ray.radius = radius;
1848
1849         bvhtree_ray_cast_data_precalc(&data, flag);
1850
1851         if (hit) {
1852                 memcpy(&data.hit, hit, sizeof(*hit));
1853         }
1854         else {
1855                 data.hit.index = -1;
1856                 data.hit.dist = BVH_RAYCAST_DIST_MAX;
1857         }
1858
1859         if (root) {
1860                 dfs_raycast(&data, root);
1861 //              iterative_raycast(&data, root);
1862         }
1863
1864
1865         if (hit)
1866                 memcpy(hit, &data.hit, sizeof(*hit));
1867
1868         return data.hit.index;
1869 }
1870
1871 int BLI_bvhtree_ray_cast(
1872         BVHTree *tree, const float co[3], const float dir[3], float radius, BVHTreeRayHit *hit,
1873         BVHTree_RayCastCallback callback, void *userdata)
1874 {
1875         return BLI_bvhtree_ray_cast_ex(tree, co, dir, radius, hit, callback, userdata, BVH_RAYCAST_DEFAULT);
1876 }
1877
1878 float BLI_bvhtree_bb_raycast(const float bv[6], const float light_start[3], const float light_end[3], float pos[3])
1879 {
1880         BVHRayCastData data;
1881         float dist;
1882
1883         data.hit.dist = BVH_RAYCAST_DIST_MAX;
1884
1885         /* get light direction */
1886         sub_v3_v3v3(data.ray.direction, light_end, light_start);
1887
1888         data.ray.radius = 0.0;
1889
1890         copy_v3_v3(data.ray.origin, light_start);
1891
1892         normalize_v3(data.ray.direction);
1893         copy_v3_v3(data.ray_dot_axis, data.ray.direction);
1894
1895         dist = ray_nearest_hit(&data, bv);
1896
1897         madd_v3_v3v3fl(pos, light_start, data.ray.direction, dist);
1898
1899         return dist;
1900
1901 }
1902
1903 /**
1904  * Calls the callback for every ray intersection
1905  *
1906  * \note Using a \a callback which resets or never sets the #BVHTreeRayHit index & dist works too,
1907  * however using this function means existing generic callbacks can be used from custom callbacks without
1908  * having to handle resetting the hit beforehand.
1909  * It also avoid redundant argument and return value which aren't meaningful when collecting multiple hits.
1910  */
1911 void BLI_bvhtree_ray_cast_all_ex(
1912         BVHTree *tree, const float co[3], const float dir[3], float radius, float hit_dist,
1913         BVHTree_RayCastCallback callback, void *userdata,
1914         int flag)
1915 {
1916         BVHRayCastData data;
1917         BVHNode *root = tree->nodes[tree->totleaf];
1918
1919         BLI_ASSERT_UNIT_V3(dir);
1920         BLI_assert(callback != NULL);
1921
1922         data.tree = tree;
1923
1924         data.callback = callback;
1925         data.userdata = userdata;
1926
1927         copy_v3_v3(data.ray.origin,    co);
1928         copy_v3_v3(data.ray.direction, dir);
1929         data.ray.radius = radius;
1930
1931         bvhtree_ray_cast_data_precalc(&data, flag);
1932
1933         data.hit.index = -1;
1934         data.hit.dist = hit_dist;
1935
1936         if (root) {
1937                 dfs_raycast_all(&data, root);
1938         }
1939 }
1940
1941 void BLI_bvhtree_ray_cast_all(
1942         BVHTree *tree, const float co[3], const float dir[3], float radius, float hit_dist,
1943         BVHTree_RayCastCallback callback, void *userdata)
1944 {
1945         BLI_bvhtree_ray_cast_all_ex(tree, co, dir, radius, hit_dist, callback, userdata, BVH_RAYCAST_DEFAULT);
1946 }
1947
1948 /** \} */
1949
1950 /* -------------------------------------------------------------------- */
1951
1952 /** \name BLI_bvhtree_range_query
1953  *
1954  * Allocs and fills an array with the indexs of node that are on the given spherical range (center, radius).
1955  * Returns the size of the array.
1956  *
1957  * \{ */
1958
1959 typedef struct RangeQueryData {
1960         BVHTree *tree;
1961         const float *center;
1962         float radius_sq;  /* squared radius */
1963
1964         int hits;
1965
1966         BVHTree_RangeQuery callback;
1967         void *userdata;
1968 } RangeQueryData;
1969
1970
1971 static void dfs_range_query(RangeQueryData *data, BVHNode *node)
1972 {
1973         if (node->totnode == 0) {
1974 #if 0   /*UNUSED*/
1975                 /* Calculate the node min-coords (if the node was a point then this is the point coordinates) */
1976                 float co[3];
1977                 co[0] = node->bv[0];
1978                 co[1] = node->bv[2];
1979                 co[2] = node->bv[4];
1980 #endif
1981         }
1982         else {
1983                 int i;
1984                 for (i = 0; i != node->totnode; i++) {
1985                         float nearest[3];
1986                         float dist_sq = calc_nearest_point_squared(data->center, node->children[i], nearest);
1987                         if (dist_sq < data->radius_sq) {
1988                                 /* Its a leaf.. call the callback */
1989                                 if (node->children[i]->totnode == 0) {
1990                                         data->hits++;
1991                                         data->callback(data->userdata, node->children[i]->index, data->center, dist_sq);
1992                                 }
1993                                 else
1994                                         dfs_range_query(data, node->children[i]);
1995                         }
1996                 }
1997         }
1998 }
1999
2000 int BLI_bvhtree_range_query(
2001         BVHTree *tree, const float co[3], float radius,
2002         BVHTree_RangeQuery callback, void *userdata)
2003 {
2004         BVHNode *root = tree->nodes[tree->totleaf];
2005
2006         RangeQueryData data;
2007         data.tree = tree;
2008         data.center = co;
2009         data.radius_sq = radius * radius;
2010         data.hits = 0;
2011
2012         data.callback = callback;
2013         data.userdata = userdata;
2014
2015         if (root != NULL) {
2016                 float nearest[3];
2017                 float dist_sq = calc_nearest_point_squared(data.center, root, nearest);
2018                 if (dist_sq < data.radius_sq) {
2019                         /* Its a leaf.. call the callback */
2020                         if (root->totnode == 0) {
2021                                 data.hits++;
2022                                 data.callback(data.userdata, root->index, co, dist_sq);
2023                         }
2024                         else
2025                                 dfs_range_query(&data, root);
2026                 }
2027         }
2028
2029         return data.hits;
2030 }
2031
2032 /** \} */
2033
2034
2035 /* -------------------------------------------------------------------- */
2036
2037 /** \name BLI_bvhtree_nearest_projected
2038 * \{ */
2039
2040 static void bvhtree_nearest_projected_dfs_recursive(
2041         BVHNearestProjectedData *__restrict data, const BVHNode *node)
2042 {
2043         if (node->totnode == 0) {
2044                 if (data->callback) {
2045                         data->callback(
2046                                 data->userdata, node->index, &data->precalc,
2047                                 NULL, 0,
2048                                 &data->nearest);
2049                 }
2050                 else {
2051                         data->nearest.index = node->index;
2052                         data->nearest.dist_sq = dist_squared_to_projected_aabb(
2053                                 &data->precalc,
2054                                 (float[3]) {node->bv[0], node->bv[2], node->bv[4]},
2055                                 (float[3]) {node->bv[1], node->bv[3], node->bv[5]},
2056                                 data->closest_axis);
2057                 }
2058         }
2059         else {
2060                 /* First pick the closest node to recurse into */
2061                 if (data->closest_axis[node->main_axis]) {
2062                         for (int i = 0; i != node->totnode; i++) {
2063                                 const float *bv = node->children[i]->bv;
2064
2065                                 if (dist_squared_to_projected_aabb(
2066                                         &data->precalc,
2067                                         (float[3]) {bv[0], bv[2], bv[4]},
2068                                         (float[3]) {bv[1], bv[3], bv[5]},
2069                                         data->closest_axis) <= data->nearest.dist_sq)
2070                                 {
2071                                         bvhtree_nearest_projected_dfs_recursive(data, node->children[i]);
2072                                 }
2073                         }
2074                 }
2075                 else {
2076                         for (int i = node->totnode; i--;) {
2077                                 const float *bv = node->children[i]->bv;
2078
2079                                 if (dist_squared_to_projected_aabb(
2080                                         &data->precalc,
2081                                         (float[3]) {bv[0], bv[2], bv[4]},
2082                                         (float[3]) {bv[1], bv[3], bv[5]},
2083                                         data->closest_axis) <= data->nearest.dist_sq)
2084                                 {
2085                                         bvhtree_nearest_projected_dfs_recursive(data, node->children[i]);
2086                                 }
2087                         }
2088                 }
2089         }
2090 }
2091
2092 static void bvhtree_nearest_projected_with_clipplane_test_dfs_recursive(
2093         BVHNearestProjectedData *__restrict data, const BVHNode *node)
2094 {
2095         if (node->totnode == 0) {
2096                 if (data->callback) {
2097                         data->callback(
2098                                 data->userdata, node->index, &data->precalc,
2099                                 data->clip_plane, data->clip_plane_len,
2100                                 &data->nearest);
2101                 }
2102                 else {
2103                         data->nearest.index = node->index;
2104                         data->nearest.dist_sq = dist_squared_to_projected_aabb(
2105                                 &data->precalc,
2106                                 (float[3]) {node->bv[0], node->bv[2], node->bv[4]},
2107                                 (float[3]) {node->bv[1], node->bv[3], node->bv[5]},
2108                                 data->closest_axis);
2109                 }
2110         }
2111         else {
2112                 /* First pick the closest node to recurse into */
2113                 if (data->closest_axis[node->main_axis]) {
2114                         for (int i = 0; i != node->totnode; i++) {
2115                                 const float *bv = node->children[i]->bv;
2116                                 const float bb_min[3] = {bv[0], bv[2], bv[4]};
2117                                 const float bb_max[3] = {bv[1], bv[3], bv[5]};
2118
2119                                 int isect_type = isect_aabb_planes_v3(data->clip_plane, data->clip_plane_len, bb_min, bb_max);
2120
2121                                 if ((isect_type != ISECT_AABB_PLANE_BEHIND_ANY) && dist_squared_to_projected_aabb(
2122                                         &data->precalc, bb_min, bb_max,
2123                                         data->closest_axis) <= data->nearest.dist_sq)
2124                                 {
2125                                         if (isect_type == ISECT_AABB_PLANE_CROSS_ANY) {
2126                                                 bvhtree_nearest_projected_with_clipplane_test_dfs_recursive(data, node->children[i]);
2127                                         }
2128                                         else {
2129                                                 /* ISECT_AABB_PLANE_IN_FRONT_ALL */
2130                                                 bvhtree_nearest_projected_dfs_recursive(data, node->children[i]);
2131                                         }
2132                                 }
2133                         }
2134                 }
2135                 else {
2136                         for (int i = node->totnode; i--;) {
2137                                 const float *bv = node->children[i]->bv;
2138                                 const float bb_min[3] = {bv[0], bv[2], bv[4]};
2139                                 const float bb_max[3] = {bv[1], bv[3], bv[5]};
2140
2141                                 int isect_type = isect_aabb_planes_v3(data->clip_plane, data->clip_plane_len, bb_min, bb_max);
2142
2143                                 if (isect_type != ISECT_AABB_PLANE_BEHIND_ANY && dist_squared_to_projected_aabb(
2144                                         &data->precalc, bb_min, bb_max,
2145                                         data->closest_axis) <= data->nearest.dist_sq)
2146                                 {
2147                                         if (isect_type == ISECT_AABB_PLANE_CROSS_ANY) {
2148                                                 bvhtree_nearest_projected_with_clipplane_test_dfs_recursive(data, node->children[i]);
2149                                         }
2150                                         else {
2151                                                 /* ISECT_AABB_PLANE_IN_FRONT_ALL */
2152                                                 bvhtree_nearest_projected_dfs_recursive(data, node->children[i]);
2153                                         }
2154                                 }
2155                         }
2156                 }
2157         }
2158 }
2159
2160 int BLI_bvhtree_find_nearest_projected(
2161         BVHTree *tree, float projmat[4][4], float winsize[2], float mval[2],
2162         float clip_plane[6][4], int clip_plane_len,
2163         BVHTreeNearest *nearest,
2164         BVHTree_NearestProjectedCallback callback, void *userdata)
2165 {
2166         BVHNode *root = tree->nodes[tree->totleaf];
2167         if (root != NULL) {
2168                 BVHNearestProjectedData data;
2169                 dist_squared_to_projected_aabb_precalc(
2170                         &data.precalc, projmat, winsize, mval);
2171
2172                 data.callback = callback;
2173                 data.userdata = userdata;
2174
2175                 if (clip_plane) {
2176                         data.clip_plane_len = clip_plane_len;
2177                         for (int i = 0; i < data.clip_plane_len; i++) {
2178                                 copy_v4_v4(data.clip_plane[i], clip_plane[i]);
2179                         }
2180                 }
2181                 else {
2182                         data.clip_plane_len = 1;
2183                         planes_from_projmat(
2184                                 projmat,
2185                                 NULL, NULL, NULL, NULL,
2186                                 data.clip_plane[0], NULL);
2187                 }
2188
2189                 if (nearest) {
2190                         memcpy(&data.nearest, nearest, sizeof(*nearest));
2191                 }
2192                 else {
2193                         data.nearest.index = -1;
2194                         data.nearest.dist_sq = FLT_MAX;
2195                 }
2196                 {
2197                         const float bb_min[3] = {root->bv[0], root->bv[2], root->bv[4]};
2198                         const float bb_max[3] = {root->bv[1], root->bv[3], root->bv[5]};
2199
2200                         int isect_type = isect_aabb_planes_v3(data.clip_plane, data.clip_plane_len, bb_min, bb_max);
2201
2202                         if (isect_type != 0 && dist_squared_to_projected_aabb(
2203                                 &data.precalc, bb_min, bb_max,
2204                                 data.closest_axis) <= data.nearest.dist_sq)
2205                         {
2206                                 if (isect_type == 1) {
2207                                         bvhtree_nearest_projected_with_clipplane_test_dfs_recursive(&data, root);
2208                                 }
2209                                 else {
2210                                         bvhtree_nearest_projected_dfs_recursive(&data, root);
2211                                 }
2212                         }
2213                 }
2214
2215                 if (nearest) {
2216                         memcpy(nearest, &data.nearest, sizeof(*nearest));
2217                 }
2218
2219                 return data.nearest.index;
2220         }
2221         return -1;
2222 }
2223
2224 /** \} */
2225
2226
2227 /* -------------------------------------------------------------------- */
2228
2229 /** \name BLI_bvhtree_walk_dfs
2230  * \{ */
2231
2232 typedef struct BVHTree_WalkData {
2233         BVHTree_WalkParentCallback walk_parent_cb;
2234         BVHTree_WalkLeafCallback walk_leaf_cb;
2235         BVHTree_WalkOrderCallback walk_order_cb;
2236         void *userdata;
2237 } BVHTree_WalkData;
2238
2239 /**
2240  * Runs first among nodes children of the first node before going to the next node in the same layer.
2241  *
2242  * \return false to break out of the search early.
2243  */
2244 static bool bvhtree_walk_dfs_recursive(
2245         BVHTree_WalkData *walk_data,
2246         const BVHNode *node)
2247 {
2248         if (node->totnode == 0) {
2249                 return walk_data->walk_leaf_cb((const BVHTreeAxisRange *)node->bv, node->index, walk_data->userdata);
2250         }
2251         else {
2252                 /* First pick the closest node to recurse into */
2253                 if (walk_data->walk_order_cb((const BVHTreeAxisRange *)node->bv, node->main_axis, walk_data->userdata)) {
2254                         for (int i = 0; i != node->totnode; i++) {
2255                                 if (walk_data->walk_parent_cb((const BVHTreeAxisRange *)node->children[i]->bv, walk_data->userdata)) {
2256                                         if (!bvhtree_walk_dfs_recursive(walk_data, node->children[i])) {
2257                                                 return false;
2258                                         }
2259                                 }
2260                         }
2261                 }
2262                 else {
2263                         for (int i = node->totnode - 1; i >= 0; i--) {
2264                                 if (walk_data->walk_parent_cb((const BVHTreeAxisRange *)node->children[i]->bv, walk_data->userdata)) {
2265                                         if (!bvhtree_walk_dfs_recursive(walk_data, node->children[i])) {
2266                                                 return false;
2267                                         }
2268                                 }
2269                         }
2270                 }
2271         }
2272         return true;
2273 }
2274
2275 /**
2276  * This is a generic function to perform a depth first search on the BVHTree
2277  * where the search order and nodes traversed depend on callbacks passed in.
2278  *
2279  * \param tree: Tree to walk.
2280  * \param walk_parent_cb: Callback on a parents bound-box to test if it should be traversed.
2281  * \param walk_leaf_cb: Callback to test leaf nodes, callback must store its own result,
2282  * returning false exits early.
2283  * \param walk_order_cb: Callback that indicates which direction to search,
2284  * either from the node with the lower or higher k-dop axis value.
2285  * \param userdata: Argument passed to all callbacks.
2286  */
2287 void BLI_bvhtree_walk_dfs(
2288         BVHTree *tree,
2289         BVHTree_WalkParentCallback walk_parent_cb,
2290         BVHTree_WalkLeafCallback walk_leaf_cb,
2291         BVHTree_WalkOrderCallback walk_order_cb, void *userdata)
2292 {
2293         const BVHNode *root = tree->nodes[tree->totleaf];
2294         if (root != NULL) {
2295                 BVHTree_WalkData walk_data = {walk_parent_cb, walk_leaf_cb, walk_order_cb, userdata};
2296                 /* first make sure the bv of root passes in the test too */
2297                 if (walk_parent_cb((const BVHTreeAxisRange *)root->bv, userdata)) {
2298                         bvhtree_walk_dfs_recursive(&walk_data, root);
2299                 }
2300         }
2301 }
2302
2303 /** \} */