svn merge -r38804:38971 https://svn.blender.org/svnroot/bf-blender/trunk/blender...
[blender.git] / source / blender / blenlib / intern / pbvh.c
1 /*
2  * $Id$
3  *
4  * ***** BEGIN GPL LICENSE BLOCK *****
5  *
6  * This program is free software; you can redistribute it and/or
7  * modify it under the terms of the GNU General Public License
8  * as published by the Free Software Foundation; either version 2
9  * of the License, or (at your option) any later version.
10  *
11  * This program is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with this program; if not, write to the Free Software Foundation,
18  * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
19  *
20  * ***** END GPL LICENSE BLOCK *****
21  */
22
23 /** \file blender/blenlib/intern/pbvh.c
24  *  \ingroup bli
25  */
26
27
28
29 #include "DNA_meshdata_types.h"
30
31 #include "MEM_guardedalloc.h"
32
33 #include "BLI_math.h"
34 #include "BLI_utildefines.h"
35 #include "BLI_ghash.h"
36 #include "BLI_pbvh.h"
37
38 #include "BKE_DerivedMesh.h"
39 #include "BKE_mesh.h" /* for mesh_calc_normals */
40 #include "BKE_global.h" /* for mesh_calc_normals */
41
42 #include "GPU_buffers.h"
43
44 #define LEAF_LIMIT 10000
45
46 //#define PERFCNTRS
47
48 /* Bitmap */
49 typedef char* BLI_bitmap;
50
51 static BLI_bitmap BLI_bitmap_new(int tot)
52 {
53         return MEM_callocN((tot >> 3) + 1, "BLI bitmap");
54 }
55
56 static int BLI_bitmap_get(BLI_bitmap b, int index)
57 {
58         return b[index >> 3] & (1 << (index & 7));
59 }
60
61 static void BLI_bitmap_set(BLI_bitmap b, int index)
62 {
63         b[index >> 3] |= (1 << (index & 7));
64 }
65
66 #if 0 /* UNUSED */
67 static void BLI_bitmap_clear(BLI_bitmap b, int index)
68 {
69         b[index >> 3] &= ~(1 << (index & 7));
70 }
71 #endif
72
73 /* Axis-aligned bounding box */
74 typedef struct {
75         float bmin[3], bmax[3];
76 } BB;
77
78 /* Axis-aligned bounding box with centroid */
79 typedef struct {
80         float bmin[3], bmax[3], bcentroid[3];
81 } BBC;
82
83 struct PBVHNode {
84         /* Opaque handle for drawing code */
85         void *draw_buffers;
86
87         /* Voxel bounds */
88         BB vb;
89         BB orig_vb;
90
91         /* For internal nodes, the offset of the children in the PBVH
92            'nodes' array. */
93         int children_offset;
94
95         /* Pointer into the PBVH prim_indices array and the number of
96            primitives used by this leaf node.
97
98            Used for leaf nodes in both mesh- and multires-based PBVHs.
99         */
100         int *prim_indices;
101         unsigned int totprim;
102
103         /* Array of indices into the mesh's MVert array. Contains the
104            indices of all vertices used by faces that are within this
105            node's bounding box.
106
107            Note that a vertex might be used by a multiple faces, and
108            these faces might be in different leaf nodes. Such a vertex
109            will appear in the vert_indices array of each of those leaf
110            nodes.
111
112            In order to support cases where you want access to multiple
113            nodes' vertices without duplication, the vert_indices array
114            is ordered such that the first part of the array, up to
115            index 'uniq_verts', contains "unique" vertex indices. These
116            vertices might not be truly unique to this node, but if
117            they appear in another node's vert_indices array, they will
118            be above that node's 'uniq_verts' value.
119
120            Used for leaf nodes in a mesh-based PBVH (not multires.)
121         */
122         int *vert_indices;
123         unsigned int uniq_verts, face_verts;
124
125         /* An array mapping face corners into the vert_indices
126            array. The array is sized to match 'totprim', and each of
127            the face's corners gets an index into the vert_indices
128            array, in the same order as the corners in the original
129            MFace. The fourth value should not be used if the original
130            face is a triangle.
131
132            Used for leaf nodes in a mesh-based PBVH (not multires.)
133         */
134         int (*face_vert_indices)[4];
135
136         /* Indicates whether this node is a leaf or not; also used for
137            marking various updates that need to be applied. */
138         PBVHNodeFlags flag : 8;
139
140         /* Used for raycasting: how close bb is to the ray point. */
141         float tmin;
142
143         int proxy_count;
144         PBVHProxyNode* proxies;
145 };
146
147 struct PBVH {
148         PBVHNode *nodes;
149         int node_mem_count, totnode;
150
151         int *prim_indices;
152         int totprim;
153         int totvert;
154
155         int leaf_limit;
156
157         /* Mesh data */
158         MVert *verts;
159         MFace *faces;
160
161         /* Grid Data */
162         DMGridData **grids;
163         DMGridAdjacency *gridadj;
164         void **gridfaces;
165         int totgrid;
166         int gridsize;
167
168         /* Only used during BVH build and update,
169            don't need to remain valid after */
170         BLI_bitmap vert_bitmap;
171
172 #ifdef PERFCNTRS
173         int perf_modified;
174 #endif
175
176         /* flag are verts/faces deformed */
177         int deformed;
178 };
179
180 #define STACK_FIXED_DEPTH       100
181
182 typedef struct PBVHStack {
183         PBVHNode *node;
184         int revisiting;
185 } PBVHStack;
186
187 typedef struct PBVHIter {
188         PBVH *bvh;
189         BLI_pbvh_SearchCallback scb;
190         void *search_data;
191
192         PBVHStack *stack;
193         int stacksize;
194
195         PBVHStack stackfixed[STACK_FIXED_DEPTH];
196         int stackspace;
197 } PBVHIter;
198
199 static void BB_reset(BB *bb)
200 {
201         bb->bmin[0] = bb->bmin[1] = bb->bmin[2] = FLT_MAX;
202         bb->bmax[0] = bb->bmax[1] = bb->bmax[2] = -FLT_MAX;
203 }
204
205 /* Expand the bounding box to include a new coordinate */
206 static void BB_expand(BB *bb, float co[3])
207 {
208         int i;
209         for(i = 0; i < 3; ++i) {
210                 bb->bmin[i] = MIN2(bb->bmin[i], co[i]);
211                 bb->bmax[i] = MAX2(bb->bmax[i], co[i]);
212         }
213 }
214
215 /* Expand the bounding box to include another bounding box */
216 static void BB_expand_with_bb(BB *bb, BB *bb2)
217 {
218         int i;
219         for(i = 0; i < 3; ++i) {
220                 bb->bmin[i] = MIN2(bb->bmin[i], bb2->bmin[i]);
221                 bb->bmax[i] = MAX2(bb->bmax[i], bb2->bmax[i]);
222         }
223 }
224
225 /* Return 0, 1, or 2 to indicate the widest axis of the bounding box */
226 static int BB_widest_axis(BB *bb)
227 {
228         float dim[3];
229         int i;
230
231         for(i = 0; i < 3; ++i)
232                 dim[i] = bb->bmax[i] - bb->bmin[i];
233
234         if(dim[0] > dim[1]) {
235                 if(dim[0] > dim[2])
236                         return 0;
237                 else
238                         return 2;
239         }
240         else {
241                 if(dim[1] > dim[2])
242                         return 1;
243                 else
244                         return 2;
245         }
246 }
247
248 static void BBC_update_centroid(BBC *bbc)
249 {
250         int i;
251         for(i = 0; i < 3; ++i)
252                 bbc->bcentroid[i] = (bbc->bmin[i] + bbc->bmax[i]) * 0.5f;
253 }
254
255 /* Not recursive */
256 static void update_node_vb(PBVH *bvh, PBVHNode *node)
257 {
258         BB vb;
259
260         BB_reset(&vb);
261         
262         if(node->flag & PBVH_Leaf) {
263                 PBVHVertexIter vd;
264
265                 BLI_pbvh_vertex_iter_begin(bvh, node, vd, PBVH_ITER_ALL) {
266                         BB_expand(&vb, vd.co);
267                 }
268                 BLI_pbvh_vertex_iter_end;
269         }
270         else {
271                 BB_expand_with_bb(&vb,
272                                   &bvh->nodes[node->children_offset].vb);
273                 BB_expand_with_bb(&vb,
274                                   &bvh->nodes[node->children_offset + 1].vb);
275         }
276
277         node->vb= vb;
278 }
279
280 //void BLI_pbvh_node_BB_reset(PBVHNode* node)
281 //{
282 //      BB_reset(&node->vb);
283 //}
284 //
285 //void BLI_pbvh_node_BB_expand(PBVHNode* node, float co[3])
286 //{
287 //      BB_expand(&node->vb, co);
288 //}
289
290
291 /* Adapted from BLI_kdopbvh.c */
292 /* Returns the index of the first element on the right of the partition */
293 static int partition_indices(int *prim_indices, int lo, int hi, int axis,
294                                  float mid, BBC *prim_bbc)
295 {
296         int i=lo, j=hi;
297         for(;;) {
298                 for(; prim_bbc[prim_indices[i]].bcentroid[axis] < mid; i++);
299                 for(; mid < prim_bbc[prim_indices[j]].bcentroid[axis]; j--);
300                 
301                 if(!(i < j))
302                         return i;
303                 
304                 SWAP(int, prim_indices[i], prim_indices[j]);
305                 i++;
306         }
307 }
308
309 static void check_partitioning(int *prim_indices, int lo, int hi, int axis,
310                                    float mid, BBC *prim_bbc, int index_of_2nd_partition)
311 {
312         int i;
313         for(i = lo; i <= hi; ++i) {
314                 const float c = prim_bbc[prim_indices[i]].bcentroid[axis];
315
316                 if((i < index_of_2nd_partition && c > mid) ||
317                    (i > index_of_2nd_partition && c < mid)) {
318                         printf("fail\n");
319                 }
320         }
321 }
322
323 static void grow_nodes(PBVH *bvh, int totnode)
324 {
325         if(totnode > bvh->node_mem_count) {
326                 PBVHNode *prev = bvh->nodes;
327                 bvh->node_mem_count *= 1.33;
328                 if(bvh->node_mem_count < totnode)
329                         bvh->node_mem_count = totnode;
330                 bvh->nodes = MEM_callocN(sizeof(PBVHNode) * bvh->node_mem_count,
331                                          "bvh nodes");
332                 memcpy(bvh->nodes, prev, bvh->totnode * sizeof(PBVHNode));
333                 MEM_freeN(prev);
334         }
335
336         bvh->totnode = totnode;
337 }
338
339 /* Add a vertex to the map, with a positive value for unique vertices and
340    a negative value for additional vertices */
341 static int map_insert_vert(PBVH *bvh, GHash *map,
342                                 unsigned int *face_verts,
343                                 unsigned int *uniq_verts, int vertex)
344 {
345         void *value, *key = SET_INT_IN_POINTER(vertex);
346
347         if(!BLI_ghash_haskey(map, key)) {
348                 if(BLI_bitmap_get(bvh->vert_bitmap, vertex)) {
349                         value = SET_INT_IN_POINTER(-(*face_verts) - 1);
350                         ++(*face_verts);
351                 }
352                 else {
353                         BLI_bitmap_set(bvh->vert_bitmap, vertex);
354                         value = SET_INT_IN_POINTER(*uniq_verts);
355                         ++(*uniq_verts);
356                 }
357                 
358                 BLI_ghash_insert(map, key, value);
359                 return GET_INT_FROM_POINTER(value);
360         }
361         else
362                 return GET_INT_FROM_POINTER(BLI_ghash_lookup(map, key));
363 }
364
365 /* Find vertices used by the faces in this node and update the draw buffers */
366 static void build_mesh_leaf_node(PBVH *bvh, PBVHNode *node)
367 {
368         GHashIterator *iter;
369         GHash *map;
370         int i, j, totface;
371
372         map = BLI_ghash_new(BLI_ghashutil_inthash, BLI_ghashutil_intcmp, "build_mesh_leaf_node gh");
373         
374         node->uniq_verts = node->face_verts = 0;
375         totface= node->totprim;
376
377         node->face_vert_indices = MEM_callocN(sizeof(int) * 4*totface,
378                                               "bvh node face vert indices");
379
380         for(i = 0; i < totface; ++i) {
381                 MFace *f = bvh->faces + node->prim_indices[i];
382                 int sides = f->v4 ? 4 : 3;
383
384                 for(j = 0; j < sides; ++j) {
385                         node->face_vert_indices[i][j]= 
386                                 map_insert_vert(bvh, map, &node->face_verts,
387                                                 &node->uniq_verts, (&f->v1)[j]);
388                 }
389         }
390
391         node->vert_indices = MEM_callocN(sizeof(int) *
392                                          (node->uniq_verts + node->face_verts),
393                                          "bvh node vert indices");
394
395         /* Build the vertex list, unique verts first */
396         for(iter = BLI_ghashIterator_new(map), i = 0;
397                 !BLI_ghashIterator_isDone(iter);
398                 BLI_ghashIterator_step(iter), ++i) {
399                 void *value = BLI_ghashIterator_getValue(iter);
400                 int ndx = GET_INT_FROM_POINTER(value);
401
402                 if(ndx < 0)
403                         ndx = -ndx + node->uniq_verts - 1;
404
405                 node->vert_indices[ndx] =
406                         GET_INT_FROM_POINTER(BLI_ghashIterator_getKey(iter));
407         }
408
409         BLI_ghashIterator_free(iter);
410
411         for(i = 0; i < totface; ++i) {
412                 MFace *f = bvh->faces + node->prim_indices[i];
413                 int sides = f->v4 ? 4 : 3;
414                 
415                 for(j = 0; j < sides; ++j) {
416                         if(node->face_vert_indices[i][j] < 0)
417                                 node->face_vert_indices[i][j]=
418                                         -node->face_vert_indices[i][j] +
419                                         node->uniq_verts - 1;
420                 }
421         }
422
423         if(!G.background) {
424                 node->draw_buffers =
425                         GPU_build_mesh_buffers(map, bvh->verts, bvh->faces,
426                                         node->prim_indices,
427                                         node->totprim, node->vert_indices,
428                                         node->uniq_verts,
429                                         node->uniq_verts + node->face_verts);
430         }
431
432         node->flag |= PBVH_UpdateDrawBuffers;
433
434         BLI_ghash_free(map, NULL, NULL);
435 }
436
437 static void build_grids_leaf_node(PBVH *bvh, PBVHNode *node)
438 {
439         if(!G.background) {
440                 node->draw_buffers =
441                         GPU_build_grid_buffers(bvh->grids, node->prim_indices,
442                                 node->totprim, bvh->gridsize);
443         }
444         node->flag |= PBVH_UpdateDrawBuffers;
445 }
446
447 /* Recursively build a node in the tree
448
449    vb is the voxel box around all of the primitives contained in
450    this node.
451
452    cb is the bounding box around all the centroids of the primitives
453    contained in this node
454
455    offset and start indicate a range in the array of primitive indices
456 */
457
458 static void build_sub(PBVH *bvh, int node_index, BB *cb, BBC *prim_bbc,
459                    int offset, int count)
460 {
461         int i, axis, end;
462         BB cb_backing;
463
464         /* Decide whether this is a leaf or not */
465         // XXX adapt leaf limit for grids
466         if(count <= bvh->leaf_limit) {
467                 bvh->nodes[node_index].flag |= PBVH_Leaf;
468
469                 bvh->nodes[node_index].prim_indices = bvh->prim_indices + offset;
470                 bvh->nodes[node_index].totprim = count;
471
472                 /* Still need vb for searches */
473                 BB_reset(&bvh->nodes[node_index].vb);
474                 for(i = offset + count - 1; i >= offset; --i) {
475                         BB_expand_with_bb(&bvh->nodes[node_index].vb,
476                                           (BB*)(prim_bbc +
477                                                 bvh->prim_indices[i]));
478                 }
479                 
480                 if(bvh->faces)
481                         build_mesh_leaf_node(bvh, bvh->nodes + node_index);
482                 else
483                         build_grids_leaf_node(bvh, bvh->nodes + node_index);
484                 bvh->nodes[node_index].orig_vb= bvh->nodes[node_index].vb;
485
486                 /* Done with this subtree */
487                 return;
488         }
489         else {
490                 BB_reset(&bvh->nodes[node_index].vb);
491                 bvh->nodes[node_index].children_offset = bvh->totnode;
492                 grow_nodes(bvh, bvh->totnode + 2);
493
494                 if(!cb) {
495                         cb = &cb_backing;
496                         BB_reset(cb);
497                         for(i = offset + count - 1; i >= offset; --i)
498                                 BB_expand(cb, prim_bbc[bvh->prim_indices[i]].bcentroid);
499                 }
500         }
501
502         axis = BB_widest_axis(cb);
503
504         for(i = offset + count - 1; i >= offset; --i) {
505                 BB_expand_with_bb(&bvh->nodes[node_index].vb,
506                                   (BB*)(prim_bbc + bvh->prim_indices[i]));
507         }
508
509         bvh->nodes[node_index].orig_vb= bvh->nodes[node_index].vb;
510
511         end = partition_indices(bvh->prim_indices, offset, offset + count - 1,
512                                 axis,
513                                 (cb->bmax[axis] + cb->bmin[axis]) * 0.5f,
514                                 prim_bbc);
515         check_partitioning(bvh->prim_indices, offset, offset + count - 1,
516                            axis,
517                            (cb->bmax[axis] + cb->bmin[axis]) * 0.5f,
518                            prim_bbc, end);
519
520         build_sub(bvh, bvh->nodes[node_index].children_offset, NULL,
521                   prim_bbc, offset, end - offset);
522         build_sub(bvh, bvh->nodes[node_index].children_offset + 1, NULL,
523                   prim_bbc, end, offset + count - end);
524 }
525
526 static void pbvh_build(PBVH *bvh, BB *cb, BBC *prim_bbc, int totprim)
527 {
528         int i;
529
530         if(totprim != bvh->totprim) {
531                 bvh->totprim = totprim;
532                 if(bvh->nodes) MEM_freeN(bvh->nodes);
533                 if(bvh->prim_indices) MEM_freeN(bvh->prim_indices);
534                 bvh->prim_indices = MEM_callocN(sizeof(int) * totprim,
535                                                 "bvh prim indices");
536                 for(i = 0; i < totprim; ++i)
537                         bvh->prim_indices[i] = i;
538                 bvh->totnode = 0;
539                 if(bvh->node_mem_count < 100) {
540                         bvh->node_mem_count = 100;
541                         bvh->nodes = MEM_callocN(sizeof(PBVHNode) *
542                                                  bvh->node_mem_count,
543                                                  "bvh initial nodes");
544                 }
545         }
546
547         bvh->totnode = 1;
548         build_sub(bvh, 0, cb, prim_bbc, 0, totprim);
549 }
550
551 /* Do a full rebuild with on Mesh data structure */
552 void BLI_pbvh_build_mesh(PBVH *bvh, MFace *faces, MVert *verts, int totface, int totvert)
553 {
554         BBC *prim_bbc = NULL;
555         BB cb;
556         int i, j;
557
558         bvh->faces = faces;
559         bvh->verts = verts;
560         bvh->vert_bitmap = BLI_bitmap_new(totvert);
561         bvh->totvert = totvert;
562         bvh->leaf_limit = LEAF_LIMIT;
563
564         BB_reset(&cb);
565
566         /* For each face, store the AABB and the AABB centroid */
567         prim_bbc = MEM_mallocN(sizeof(BBC) * totface, "prim_bbc");
568
569         for(i = 0; i < totface; ++i) {
570                 MFace *f = faces + i;
571                 const int sides = f->v4 ? 4 : 3;
572                 BBC *bbc = prim_bbc + i;
573
574                 BB_reset((BB*)bbc);
575
576                 for(j = 0; j < sides; ++j)
577                         BB_expand((BB*)bbc, verts[(&f->v1)[j]].co);
578
579                 BBC_update_centroid(bbc);
580
581                 BB_expand(&cb, bbc->bcentroid);
582         }
583
584         if(totface)
585                 pbvh_build(bvh, &cb, prim_bbc, totface);
586
587         MEM_freeN(prim_bbc);
588         MEM_freeN(bvh->vert_bitmap);
589 }
590
591 /* Do a full rebuild with on Grids data structure */
592 void BLI_pbvh_build_grids(PBVH *bvh, DMGridData **grids, DMGridAdjacency *gridadj,
593         int totgrid, int gridsize, void **gridfaces)
594 {
595         BBC *prim_bbc = NULL;
596         BB cb;
597         int i, j;
598
599         bvh->grids= grids;
600         bvh->gridadj= gridadj;
601         bvh->gridfaces= gridfaces;
602         bvh->totgrid= totgrid;
603         bvh->gridsize= gridsize;
604         bvh->leaf_limit = MAX2(LEAF_LIMIT/((gridsize-1)*(gridsize-1)), 1);
605
606         BB_reset(&cb);
607
608         /* For each grid, store the AABB and the AABB centroid */
609         prim_bbc = MEM_mallocN(sizeof(BBC) * totgrid, "prim_bbc");
610
611         for(i = 0; i < totgrid; ++i) {
612                 DMGridData *grid= grids[i];
613                 BBC *bbc = prim_bbc + i;
614
615                 BB_reset((BB*)bbc);
616
617                 for(j = 0; j < gridsize*gridsize; ++j)
618                         BB_expand((BB*)bbc, grid[j].co);
619
620                 BBC_update_centroid(bbc);
621
622                 BB_expand(&cb, bbc->bcentroid);
623         }
624
625         if(totgrid)
626                 pbvh_build(bvh, &cb, prim_bbc, totgrid);
627
628         MEM_freeN(prim_bbc);
629 }
630
631 PBVH *BLI_pbvh_new(void)
632 {
633         PBVH *bvh = MEM_callocN(sizeof(PBVH), "pbvh");
634
635         return bvh;
636 }
637
638 void BLI_pbvh_free(PBVH *bvh)
639 {
640         PBVHNode *node;
641         int i;
642
643         for(i = 0; i < bvh->totnode; ++i) {
644                 node= &bvh->nodes[i];
645
646                 if(node->flag & PBVH_Leaf) {
647                         if(node->draw_buffers)
648                                 GPU_free_buffers(node->draw_buffers);
649                         if(node->vert_indices)
650                                 MEM_freeN(node->vert_indices);
651                         if(node->face_vert_indices)
652                                 MEM_freeN(node->face_vert_indices);
653                 }
654         }
655
656         if (bvh->deformed) {
657                 if (bvh->verts) {
658                         /* if pbvh was deformed, new memory was allocated for verts/faces -- free it */
659
660                         MEM_freeN(bvh->verts);
661                         MEM_freeN(bvh->faces);
662                 }
663         }
664
665         MEM_freeN(bvh->nodes);
666         MEM_freeN(bvh->prim_indices);
667         MEM_freeN(bvh);
668 }
669
670 static void pbvh_iter_begin(PBVHIter *iter, PBVH *bvh, BLI_pbvh_SearchCallback scb, void *search_data)
671 {
672         iter->bvh= bvh;
673         iter->scb= scb;
674         iter->search_data= search_data;
675
676         iter->stack= iter->stackfixed;
677         iter->stackspace= STACK_FIXED_DEPTH;
678
679         iter->stack[0].node= bvh->nodes;
680         iter->stack[0].revisiting= 0;
681         iter->stacksize= 1;
682 }
683
684 static void pbvh_iter_end(PBVHIter *iter)
685 {
686         if(iter->stackspace > STACK_FIXED_DEPTH)
687                 MEM_freeN(iter->stack);
688 }
689
690 static void pbvh_stack_push(PBVHIter *iter, PBVHNode *node, int revisiting)
691 {
692         if(iter->stacksize == iter->stackspace) {
693                 PBVHStack *newstack;
694
695                 iter->stackspace *= 2;
696                 newstack= MEM_callocN(sizeof(PBVHStack)*iter->stackspace, "PBVHStack");
697                 memcpy(newstack, iter->stack, sizeof(PBVHStack)*iter->stacksize);
698
699                 if(iter->stackspace > STACK_FIXED_DEPTH)
700                         MEM_freeN(iter->stack);
701                 iter->stack= newstack;
702         }
703
704         iter->stack[iter->stacksize].node= node;
705         iter->stack[iter->stacksize].revisiting= revisiting;
706         iter->stacksize++;
707 }
708
709 static PBVHNode *pbvh_iter_next(PBVHIter *iter)
710 {
711         PBVHNode *node;
712         int revisiting;
713
714         /* purpose here is to traverse tree, visiting child nodes before their
715            parents, this order is necessary for e.g. computing bounding boxes */
716
717         while(iter->stacksize) {
718                 /* pop node */
719                 iter->stacksize--;
720                 node= iter->stack[iter->stacksize].node;
721
722                 /* on a mesh with no faces this can happen
723                  * can remove this check if we know meshes have at least 1 face */
724                 if(node==NULL)
725                         return NULL;
726
727                 revisiting= iter->stack[iter->stacksize].revisiting;
728
729                 /* revisiting node already checked */
730                 if(revisiting)
731                         return node;
732
733                 if(iter->scb && !iter->scb(node, iter->search_data))
734                         continue; /* don't traverse, outside of search zone */
735
736                 if(node->flag & PBVH_Leaf) {
737                         /* immediately hit leaf node */
738                         return node;
739                 }
740                 else {
741                         /* come back later when children are done */
742                         pbvh_stack_push(iter, node, 1);
743
744                         /* push two child nodes on the stack */
745                         pbvh_stack_push(iter, iter->bvh->nodes+node->children_offset+1, 0);
746                         pbvh_stack_push(iter, iter->bvh->nodes+node->children_offset, 0);
747                 }
748         }
749
750         return NULL;
751 }
752
753 static PBVHNode *pbvh_iter_next_occluded(PBVHIter *iter)
754 {
755         PBVHNode *node;
756
757         while(iter->stacksize) {
758                 /* pop node */
759                 iter->stacksize--;
760                 node= iter->stack[iter->stacksize].node;
761
762                 /* on a mesh with no faces this can happen
763                  * can remove this check if we know meshes have at least 1 face */
764                 if(node==NULL) return NULL;
765
766                 if(iter->scb && !iter->scb(node, iter->search_data)) continue; /* don't traverse, outside of search zone */
767
768                 if(node->flag & PBVH_Leaf) {
769                         /* immediately hit leaf node */
770                         return node;
771                 }
772                 else {
773                         pbvh_stack_push(iter, iter->bvh->nodes+node->children_offset+1, 0);
774                         pbvh_stack_push(iter, iter->bvh->nodes+node->children_offset, 0);
775                 }
776         }
777
778         return NULL;
779 }
780
781 void BLI_pbvh_search_gather(PBVH *bvh,
782         BLI_pbvh_SearchCallback scb, void *search_data,
783         PBVHNode ***r_array, int *r_tot)
784 {
785         PBVHIter iter;
786         PBVHNode **array= NULL, **newarray, *node;
787         int tot= 0, space= 0;
788
789         pbvh_iter_begin(&iter, bvh, scb, search_data);
790
791         while((node=pbvh_iter_next(&iter))) {
792                 if(node->flag & PBVH_Leaf) {
793                         if(tot == space) {
794                                 /* resize array if needed */
795                                 space= (tot == 0)? 32: space*2;
796                                 newarray= MEM_callocN(sizeof(PBVHNode)*space, "PBVHNodeSearch");
797
798                                 if(array) {
799                                         memcpy(newarray, array, sizeof(PBVHNode)*tot);
800                                         MEM_freeN(array);
801                                 }
802
803                                 array= newarray;
804                         }
805
806                         array[tot]= node;
807                         tot++;
808                 }
809         }
810
811         pbvh_iter_end(&iter);
812
813         if(tot == 0 && array) {
814                 MEM_freeN(array);
815                 array= NULL;
816         }
817
818         *r_array= array;
819         *r_tot= tot;
820 }
821
822 void BLI_pbvh_search_callback(PBVH *bvh,
823         BLI_pbvh_SearchCallback scb, void *search_data,
824         BLI_pbvh_HitCallback hcb, void *hit_data)
825 {
826         PBVHIter iter;
827         PBVHNode *node;
828
829         pbvh_iter_begin(&iter, bvh, scb, search_data);
830
831         while((node=pbvh_iter_next(&iter)))
832                 if (node->flag & PBVH_Leaf)
833                         hcb(node, hit_data);
834
835         pbvh_iter_end(&iter);
836 }
837
838 typedef struct node_tree {
839         PBVHNode* data;
840
841         struct node_tree* left;
842         struct node_tree* right;
843 } node_tree;
844
845 static void node_tree_insert(node_tree* tree, node_tree* new_node)
846 {
847         if (new_node->data->tmin < tree->data->tmin) {
848                 if (tree->left) {
849                         node_tree_insert(tree->left, new_node);
850                 }
851                 else {
852                         tree->left = new_node;
853                 }
854         }
855         else {
856                 if (tree->right) {
857                         node_tree_insert(tree->right, new_node);
858                 }
859                 else {
860                         tree->right = new_node;
861                 }
862         }
863 }
864
865 static void traverse_tree(node_tree* tree, BLI_pbvh_HitOccludedCallback hcb, void* hit_data, float* tmin)
866 {
867         if (tree->left) traverse_tree(tree->left, hcb, hit_data, tmin);
868
869         hcb(tree->data, hit_data, tmin);
870
871         if (tree->right) traverse_tree(tree->right, hcb, hit_data, tmin);
872 }
873
874 static void free_tree(node_tree* tree)
875 {
876         if (tree->left) {
877                 free_tree(tree->left);
878                 tree->left = 0;
879         }
880
881         if (tree->right) {
882                 free_tree(tree->right);
883                 tree->right = 0;
884         }
885
886         free(tree);
887 }
888
889 float BLI_pbvh_node_get_tmin(PBVHNode* node)
890 {
891         return node->tmin;
892 }
893
894 static void BLI_pbvh_search_callback_occluded(PBVH *bvh,
895         BLI_pbvh_SearchCallback scb, void *search_data,
896         BLI_pbvh_HitOccludedCallback hcb, void *hit_data)
897 {
898         PBVHIter iter;
899         PBVHNode *node;
900         node_tree *tree = 0;
901
902         pbvh_iter_begin(&iter, bvh, scb, search_data);
903
904         while((node=pbvh_iter_next_occluded(&iter))) {
905                 if(node->flag & PBVH_Leaf) {
906                         node_tree* new_node = malloc(sizeof(node_tree));
907
908                         new_node->data = node;
909
910                         new_node->left  = NULL;
911                         new_node->right = NULL;
912
913                         if (tree) {
914                                 node_tree_insert(tree, new_node);
915                         }
916                         else {
917                                 tree = new_node;
918                         }
919                 }
920         }
921
922         pbvh_iter_end(&iter);
923
924         if (tree) {
925                 float tmin = FLT_MAX;
926                 traverse_tree(tree, hcb, hit_data, &tmin);
927                 free_tree(tree);
928         }
929 }
930
931 static int update_search_cb(PBVHNode *node, void *data_v)
932 {
933         int flag= GET_INT_FROM_POINTER(data_v);
934
935         if(node->flag & PBVH_Leaf)
936                 return (node->flag & flag);
937         
938         return 1;
939 }
940
941 static void pbvh_update_normals(PBVH *bvh, PBVHNode **nodes,
942         int totnode, float (*face_nors)[3])
943 {
944         float (*vnor)[3];
945         int n;
946
947         if(bvh->grids)
948                 return;
949
950         /* could be per node to save some memory, but also means
951            we have to store for each vertex which node it is in */
952         vnor= MEM_callocN(sizeof(float)*3*bvh->totvert, "bvh temp vnors");
953
954         /* subtle assumptions:
955            - We know that for all edited vertices, the nodes with faces
956                  adjacent to these vertices have been marked with PBVH_UpdateNormals.
957                  This is true because if the vertex is inside the brush radius, the
958                  bounding box of it's adjacent faces will be as well.
959            - However this is only true for the vertices that have actually been
960                  edited, not for all vertices in the nodes marked for update, so we
961                  can only update vertices marked with ME_VERT_PBVH_UPDATE.
962         */
963
964         #pragma omp parallel for private(n) schedule(static)
965         for(n = 0; n < totnode; n++) {
966                 PBVHNode *node= nodes[n];
967
968                 if((node->flag & PBVH_UpdateNormals)) {
969                         int i, j, totface, *faces;
970
971                         faces= node->prim_indices;
972                         totface= node->totprim;
973
974                         for(i = 0; i < totface; ++i) {
975                                 MFace *f= bvh->faces + faces[i];
976                                 float fn[3];
977                                 unsigned int *fv = &f->v1;
978                                 int sides= (f->v4)? 4: 3;
979
980                                 if(f->v4)
981                                         normal_quad_v3(fn, bvh->verts[f->v1].co, bvh->verts[f->v2].co,
982                                                                    bvh->verts[f->v3].co, bvh->verts[f->v4].co);
983                                 else
984                                         normal_tri_v3(fn, bvh->verts[f->v1].co, bvh->verts[f->v2].co,
985                                                                   bvh->verts[f->v3].co);
986
987                                 for(j = 0; j < sides; ++j) {
988                                         int v= fv[j];
989
990                                         if(bvh->verts[v].flag & ME_VERT_PBVH_UPDATE) {
991                                                 /* this seems like it could be very slow but profile
992                                                    does not show this, so just leave it for now? */
993                                                 #pragma omp atomic
994                                                 vnor[v][0] += fn[0];
995                                                 #pragma omp atomic
996                                                 vnor[v][1] += fn[1];
997                                                 #pragma omp atomic
998                                                 vnor[v][2] += fn[2];
999                                         }
1000                                 }
1001
1002                                 if(face_nors)
1003                                         copy_v3_v3(face_nors[faces[i]], fn);
1004                         }
1005                 }
1006         }
1007
1008         #pragma omp parallel for private(n) schedule(static)
1009         for(n = 0; n < totnode; n++) {
1010                 PBVHNode *node= nodes[n];
1011
1012                 if(node->flag & PBVH_UpdateNormals) {
1013                         int i, *verts, totvert;
1014
1015                         verts= node->vert_indices;
1016                         totvert= node->uniq_verts;
1017
1018                         for(i = 0; i < totvert; ++i) {
1019                                 const int v = verts[i];
1020                                 MVert *mvert= &bvh->verts[v];
1021
1022                                 if(mvert->flag & ME_VERT_PBVH_UPDATE) {
1023                                         float no[3];
1024
1025                                         copy_v3_v3(no, vnor[v]);
1026                                         normalize_v3(no);
1027                                         
1028                                         mvert->no[0] = (short)(no[0]*32767.0f);
1029                                         mvert->no[1] = (short)(no[1]*32767.0f);
1030                                         mvert->no[2] = (short)(no[2]*32767.0f);
1031                                         
1032                                         mvert->flag &= ~ME_VERT_PBVH_UPDATE;
1033                                 }
1034                         }
1035
1036                         node->flag &= ~PBVH_UpdateNormals;
1037                 }
1038         }
1039
1040         MEM_freeN(vnor);
1041 }
1042
1043 static void pbvh_update_BB_redraw(PBVH *bvh, PBVHNode **nodes,
1044         int totnode, int flag)
1045 {
1046         int n;
1047
1048         /* update BB, redraw flag */
1049         #pragma omp parallel for private(n) schedule(static)
1050         for(n = 0; n < totnode; n++) {
1051                 PBVHNode *node= nodes[n];
1052
1053                 if((flag & PBVH_UpdateBB) && (node->flag & PBVH_UpdateBB))
1054                         /* don't clear flag yet, leave it for flushing later */
1055                         update_node_vb(bvh, node);
1056
1057                 if((flag & PBVH_UpdateOriginalBB) && (node->flag & PBVH_UpdateOriginalBB))
1058                         node->orig_vb= node->vb;
1059
1060                 if((flag & PBVH_UpdateRedraw) && (node->flag & PBVH_UpdateRedraw))
1061                         node->flag &= ~PBVH_UpdateRedraw;
1062         }
1063 }
1064
1065 static void pbvh_update_draw_buffers(PBVH *bvh, PBVHNode **nodes, int totnode, int smooth)
1066 {
1067         PBVHNode *node;
1068         int n;
1069
1070         /* can't be done in parallel with OpenGL */
1071         for(n = 0; n < totnode; n++) {
1072                 node= nodes[n];
1073
1074                 if(node->flag & PBVH_UpdateDrawBuffers) {
1075                         if(bvh->grids) {
1076                                 GPU_update_grid_buffers(node->draw_buffers,
1077                                                    bvh->grids,
1078                                                    node->prim_indices,
1079                                                    node->totprim,
1080                                                    bvh->gridsize,
1081                                                    smooth);
1082                         }
1083                         else {
1084                                 GPU_update_mesh_buffers(node->draw_buffers,
1085                                                    bvh->verts,
1086                                                    node->vert_indices,
1087                                                    node->uniq_verts +
1088                                                    node->face_verts);
1089                         }
1090
1091                         node->flag &= ~PBVH_UpdateDrawBuffers;
1092                 }
1093         }
1094 }
1095
1096 static int pbvh_flush_bb(PBVH *bvh, PBVHNode *node, int flag)
1097 {
1098         int update= 0;
1099
1100         /* difficult to multithread well, we just do single threaded recursive */
1101         if(node->flag & PBVH_Leaf) {
1102                 if(flag & PBVH_UpdateBB) {
1103                         update |= (node->flag & PBVH_UpdateBB);
1104                         node->flag &= ~PBVH_UpdateBB;
1105                 }
1106
1107                 if(flag & PBVH_UpdateOriginalBB) {
1108                         update |= (node->flag & PBVH_UpdateOriginalBB);
1109                         node->flag &= ~PBVH_UpdateOriginalBB;
1110                 }
1111
1112                 return update;
1113         }
1114         else {
1115                 update |= pbvh_flush_bb(bvh, bvh->nodes + node->children_offset, flag);
1116                 update |= pbvh_flush_bb(bvh, bvh->nodes + node->children_offset + 1, flag);
1117
1118                 if(update & PBVH_UpdateBB)
1119                         update_node_vb(bvh, node);
1120                 if(update & PBVH_UpdateOriginalBB)
1121                         node->orig_vb= node->vb;
1122         }
1123
1124         return update;
1125 }
1126
1127 void BLI_pbvh_update(PBVH *bvh, int flag, float (*face_nors)[3])
1128 {
1129         PBVHNode **nodes;
1130         int totnode;
1131
1132         BLI_pbvh_search_gather(bvh, update_search_cb, SET_INT_IN_POINTER(flag),
1133                 &nodes, &totnode);
1134
1135         if(flag & PBVH_UpdateNormals)
1136                 pbvh_update_normals(bvh, nodes, totnode, face_nors);
1137
1138         if(flag & (PBVH_UpdateBB|PBVH_UpdateOriginalBB|PBVH_UpdateRedraw))
1139                 pbvh_update_BB_redraw(bvh, nodes, totnode, flag);
1140
1141         if(flag & (PBVH_UpdateBB|PBVH_UpdateOriginalBB))
1142                 pbvh_flush_bb(bvh, bvh->nodes, flag);
1143
1144         if(nodes) MEM_freeN(nodes);
1145 }
1146
1147 void BLI_pbvh_redraw_BB(PBVH *bvh, float bb_min[3], float bb_max[3])
1148 {
1149         PBVHIter iter;
1150         PBVHNode *node;
1151         BB bb;
1152
1153         BB_reset(&bb);
1154
1155         pbvh_iter_begin(&iter, bvh, NULL, NULL);
1156
1157         while((node=pbvh_iter_next(&iter)))
1158                 if(node->flag & PBVH_UpdateRedraw)
1159                         BB_expand_with_bb(&bb, &node->vb);
1160
1161         pbvh_iter_end(&iter);
1162
1163         copy_v3_v3(bb_min, bb.bmin);
1164         copy_v3_v3(bb_max, bb.bmax);
1165 }
1166
1167 void BLI_pbvh_get_grid_updates(PBVH *bvh, int clear, void ***gridfaces, int *totface)
1168 {
1169         PBVHIter iter;
1170         PBVHNode *node;
1171         GHashIterator *hiter;
1172         GHash *map;
1173         void *face, **faces;
1174         unsigned i;
1175         int tot;
1176
1177         map = BLI_ghash_new(BLI_ghashutil_ptrhash, BLI_ghashutil_ptrcmp, "pbvh_get_grid_updates gh");
1178
1179         pbvh_iter_begin(&iter, bvh, NULL, NULL);
1180
1181         while((node=pbvh_iter_next(&iter))) {
1182                 if(node->flag & PBVH_UpdateNormals) {
1183                         for(i = 0; i < node->totprim; ++i) {
1184                                 face= bvh->gridfaces[node->prim_indices[i]];
1185                                 if(!BLI_ghash_lookup(map, face))
1186                                         BLI_ghash_insert(map, face, face);
1187                         }
1188
1189                         if(clear)
1190                                 node->flag &= ~PBVH_UpdateNormals;
1191                 }
1192         }
1193
1194         pbvh_iter_end(&iter);
1195         
1196         tot= BLI_ghash_size(map);
1197         if(tot == 0) {
1198                 *totface= 0;
1199                 *gridfaces= NULL;
1200                 BLI_ghash_free(map, NULL, NULL);
1201                 return;
1202         }
1203
1204         faces= MEM_callocN(sizeof(void*)*tot, "PBVH Grid Faces");
1205
1206         for(hiter = BLI_ghashIterator_new(map), i = 0;
1207                 !BLI_ghashIterator_isDone(hiter);
1208                 BLI_ghashIterator_step(hiter), ++i)
1209                 faces[i]= BLI_ghashIterator_getKey(hiter);
1210
1211         BLI_ghashIterator_free(hiter);
1212
1213         BLI_ghash_free(map, NULL, NULL);
1214
1215         *totface= tot;
1216         *gridfaces= faces;
1217 }
1218
1219 /***************************** Node Access ***********************************/
1220
1221 void BLI_pbvh_node_mark_update(PBVHNode *node)
1222 {
1223         node->flag |= PBVH_UpdateNormals|PBVH_UpdateBB|PBVH_UpdateOriginalBB|PBVH_UpdateDrawBuffers|PBVH_UpdateRedraw;
1224 }
1225
1226 void BLI_pbvh_node_get_verts(PBVH *bvh, PBVHNode *node, int **vert_indices, MVert **verts)
1227 {
1228         if(vert_indices) *vert_indices= node->vert_indices;
1229         if(verts) *verts= bvh->verts;
1230 }
1231
1232 void BLI_pbvh_node_num_verts(PBVH *bvh, PBVHNode *node, int *uniquevert, int *totvert)
1233 {
1234         if(bvh->grids) {
1235                 const int tot= node->totprim*bvh->gridsize*bvh->gridsize;
1236                 if(totvert) *totvert= tot;
1237                 if(uniquevert) *uniquevert= tot;
1238         }
1239         else {
1240                 if(totvert) *totvert= node->uniq_verts + node->face_verts;
1241                 if(uniquevert) *uniquevert= node->uniq_verts;
1242         }
1243 }
1244
1245 void BLI_pbvh_node_get_grids(PBVH *bvh, PBVHNode *node, int **grid_indices, int *totgrid, int *maxgrid, int *gridsize, DMGridData ***griddata, DMGridAdjacency **gridadj)
1246 {
1247         if(bvh->grids) {
1248                 if(grid_indices) *grid_indices= node->prim_indices;
1249                 if(totgrid) *totgrid= node->totprim;
1250                 if(maxgrid) *maxgrid= bvh->totgrid;
1251                 if(gridsize) *gridsize= bvh->gridsize;
1252                 if(griddata) *griddata= bvh->grids;
1253                 if(gridadj) *gridadj= bvh->gridadj;
1254         }
1255         else {
1256                 if(grid_indices) *grid_indices= NULL;
1257                 if(totgrid) *totgrid= 0;
1258                 if(maxgrid) *maxgrid= 0;
1259                 if(gridsize) *gridsize= 0;
1260                 if(griddata) *griddata= NULL;
1261                 if(gridadj) *gridadj= NULL;
1262         }
1263 }
1264
1265 void BLI_pbvh_node_get_BB(PBVHNode *node, float bb_min[3], float bb_max[3])
1266 {
1267         copy_v3_v3(bb_min, node->vb.bmin);
1268         copy_v3_v3(bb_max, node->vb.bmax);
1269 }
1270
1271 void BLI_pbvh_node_get_original_BB(PBVHNode *node, float bb_min[3], float bb_max[3])
1272 {
1273         copy_v3_v3(bb_min, node->orig_vb.bmin);
1274         copy_v3_v3(bb_max, node->orig_vb.bmax);
1275 }
1276
1277 void BLI_pbvh_node_get_proxies(PBVHNode* node, PBVHProxyNode** proxies, int* proxy_count)
1278 {
1279         if (node->proxy_count > 0) {
1280                 if (proxies) *proxies = node->proxies;
1281                 if (proxy_count) *proxy_count = node->proxy_count;
1282         }
1283         else {
1284                 if (proxies) *proxies = 0;
1285                 if (proxy_count) *proxy_count = 0;
1286         }
1287 }
1288
1289 /********************************* Raycast ***********************************/
1290
1291 typedef struct {
1292         /* Ray */
1293         float start[3];
1294         int sign[3];
1295         float inv_dir[3];
1296         int original;
1297 } RaycastData;
1298
1299 /* Adapted from here: http://www.gamedev.net/community/forums/topic.asp?topic_id=459973 */
1300 static int ray_aabb_intersect(PBVHNode *node, void *data_v)
1301 {
1302         RaycastData *ray = data_v;
1303         float bbox[2][3];
1304         float tmin, tmax, tymin, tymax, tzmin, tzmax;
1305
1306         if(ray->original)
1307                 BLI_pbvh_node_get_original_BB(node, bbox[0], bbox[1]);
1308         else
1309                 BLI_pbvh_node_get_BB(node, bbox[0], bbox[1]);
1310
1311         tmin = (bbox[ray->sign[0]][0] - ray->start[0]) * ray->inv_dir[0];
1312         tmax = (bbox[1-ray->sign[0]][0] - ray->start[0]) * ray->inv_dir[0];
1313
1314         tymin = (bbox[ray->sign[1]][1] - ray->start[1]) * ray->inv_dir[1];
1315         tymax = (bbox[1-ray->sign[1]][1] - ray->start[1]) * ray->inv_dir[1];
1316
1317         if((tmin > tymax) || (tymin > tmax))
1318                 return 0;
1319
1320         if(tymin > tmin)
1321                 tmin = tymin;
1322
1323         if(tymax < tmax)
1324                 tmax = tymax;
1325
1326         tzmin = (bbox[ray->sign[2]][2] - ray->start[2]) * ray->inv_dir[2];
1327         tzmax = (bbox[1-ray->sign[2]][2] - ray->start[2]) * ray->inv_dir[2];
1328
1329         if((tmin > tzmax) || (tzmin > tmax))
1330                 return 0;
1331
1332         if(tzmin > tmin)
1333                 tmin = tzmin;
1334
1335         // XXX jwilkins: tmax does not need to be updated since we don't use it
1336         // keeping this here for future reference
1337         //if(tzmax < tmax) tmax = tzmax; 
1338
1339         node->tmin = tmin;
1340
1341         return 1;
1342 }
1343
1344 void BLI_pbvh_raycast(PBVH *bvh, BLI_pbvh_HitOccludedCallback cb, void *data,
1345                           float ray_start[3], float ray_normal[3], int original)
1346 {
1347         RaycastData rcd;
1348
1349         copy_v3_v3(rcd.start, ray_start);
1350         rcd.inv_dir[0] = 1.0f / ray_normal[0];
1351         rcd.inv_dir[1] = 1.0f / ray_normal[1];
1352         rcd.inv_dir[2] = 1.0f / ray_normal[2];
1353         rcd.sign[0] = rcd.inv_dir[0] < 0;
1354         rcd.sign[1] = rcd.inv_dir[1] < 0;
1355         rcd.sign[2] = rcd.inv_dir[2] < 0;
1356         rcd.original = original;
1357
1358         BLI_pbvh_search_callback_occluded(bvh, ray_aabb_intersect, &rcd, cb, data);
1359 }
1360
1361 static int ray_face_intersection(float ray_start[3], float ray_normal[3],
1362                                  float *t0, float *t1, float *t2, float *t3,
1363                                  float *fdist)
1364 {
1365         float dist;
1366
1367         if ((isect_ray_tri_epsilon_v3(ray_start, ray_normal, t0, t1, t2, &dist, NULL, 0.1f) && dist < *fdist) ||
1368            (t3 && isect_ray_tri_epsilon_v3(ray_start, ray_normal, t0, t2, t3, &dist, NULL, 0.1f) && dist < *fdist))
1369         {
1370                 *fdist = dist;
1371                 return 1;
1372         }
1373         else {
1374                 return 0;
1375         }
1376 }
1377
1378 int BLI_pbvh_node_raycast(PBVH *bvh, PBVHNode *node, float (*origco)[3],
1379         float ray_start[3], float ray_normal[3], float *dist)
1380 {
1381         int hit= 0;
1382
1383         if(bvh->faces) {
1384                 MVert *vert = bvh->verts;
1385                 int *faces= node->prim_indices;
1386                 int totface= node->totprim;
1387                 int i;
1388
1389                 for(i = 0; i < totface; ++i) {
1390                         MFace *f = bvh->faces + faces[i];
1391                         int *face_verts = node->face_vert_indices[i];
1392
1393                         if(origco) {
1394                                 /* intersect with backuped original coordinates */
1395                                 hit |= ray_face_intersection(ray_start, ray_normal,
1396                                                          origco[face_verts[0]],
1397                                                          origco[face_verts[1]],
1398                                                          origco[face_verts[2]],
1399                                                          f->v4? origco[face_verts[3]]: NULL,
1400                                                          dist);
1401                         }
1402                         else {
1403                                 /* intersect with current coordinates */
1404                                 hit |= ray_face_intersection(ray_start, ray_normal,
1405                                                          vert[f->v1].co,
1406                                                          vert[f->v2].co,
1407                                                          vert[f->v3].co,
1408                                                          f->v4 ? vert[f->v4].co : NULL,
1409                                                          dist);
1410                         }
1411                 }
1412         }
1413         else {
1414                 int totgrid= node->totprim;
1415                 int gridsize= bvh->gridsize;
1416                 int i, x, y;
1417
1418                 for(i = 0; i < totgrid; ++i) {
1419                         DMGridData *grid= bvh->grids[node->prim_indices[i]];
1420                         if (!grid)
1421                                 continue;
1422
1423                         for(y = 0; y < gridsize-1; ++y) {
1424                                 for(x = 0; x < gridsize-1; ++x) {
1425                                         if(origco) {
1426                                                 hit |= ray_face_intersection(ray_start, ray_normal,
1427                                                                          origco[y*gridsize + x],
1428                                                                          origco[y*gridsize + x+1],
1429                                                                          origco[(y+1)*gridsize + x+1],
1430                                                                          origco[(y+1)*gridsize + x],
1431                                                                          dist);
1432                                         }
1433                                         else {
1434                                                 hit |= ray_face_intersection(ray_start, ray_normal,
1435                                                                          grid[y*gridsize + x].co,
1436                                                                          grid[y*gridsize + x+1].co,
1437                                                                          grid[(y+1)*gridsize + x+1].co,
1438                                                                          grid[(y+1)*gridsize + x].co,
1439                                                                          dist);
1440                                         }
1441                                 }
1442                         }
1443
1444                         if(origco)
1445                                 origco += gridsize*gridsize;
1446                 }
1447         }
1448
1449         return hit;
1450 }
1451
1452 //#include <GL/glew.h>
1453
1454 void BLI_pbvh_node_draw(PBVHNode *node, void *UNUSED(data))
1455 {
1456 #if 0
1457         /* XXX: Just some quick code to show leaf nodes in different colors */
1458         float col[3]; int i;
1459
1460         if(0) { //is_partial) {
1461                 col[0] = (rand() / (float)RAND_MAX); col[1] = col[2] = 0.6;
1462         }
1463         else {
1464                 srand((long long)node);
1465                 for(i = 0; i < 3; ++i)
1466                         col[i] = (rand() / (float)RAND_MAX) * 0.3 + 0.7;
1467         }
1468         glMaterialfv(GL_FRONT_AND_BACK, GL_DIFFUSE, col);
1469
1470         glColor3f(1, 0, 0);
1471 #endif
1472         GPU_draw_buffers(node->draw_buffers);
1473 }
1474
1475 /* Adapted from:
1476    http://www.gamedev.net/community/forums/topic.asp?topic_id=512123
1477    Returns true if the AABB is at least partially within the frustum
1478    (ok, not a real frustum), false otherwise.
1479 */
1480 int BLI_pbvh_node_planes_contain_AABB(PBVHNode *node, void *data)
1481 {
1482         float (*planes)[4] = data;
1483         int i, axis;
1484         float vmin[3] /*, vmax[3]*/, bb_min[3], bb_max[3];
1485
1486         BLI_pbvh_node_get_BB(node, bb_min, bb_max);
1487
1488         for(i = 0; i < 4; ++i) { 
1489                 for(axis = 0; axis < 3; ++axis) {
1490                         if(planes[i][axis] > 0) { 
1491                                 vmin[axis] = bb_min[axis];
1492                                 /*vmax[axis] = bb_max[axis];*/ /*UNUSED*/
1493                         }
1494                         else {
1495                                 vmin[axis] = bb_max[axis];
1496                                 /*vmax[axis] = bb_min[axis];*/ /*UNUSED*/
1497                         }
1498                 }
1499                 
1500                 if(dot_v3v3(planes[i], vmin) + planes[i][3] > 0)
1501                         return 0;
1502         } 
1503
1504         return 1;
1505 }
1506
1507 void BLI_pbvh_draw(PBVH *bvh, float (*planes)[4], float (*face_nors)[3], int smooth)
1508 {
1509         PBVHNode **nodes;
1510         int totnode;
1511
1512         BLI_pbvh_search_gather(bvh, update_search_cb, SET_INT_IN_POINTER(PBVH_UpdateNormals|PBVH_UpdateDrawBuffers),
1513                 &nodes, &totnode);
1514
1515         pbvh_update_normals(bvh, nodes, totnode, face_nors);
1516         pbvh_update_draw_buffers(bvh, nodes, totnode, smooth);
1517
1518         if(nodes) MEM_freeN(nodes);
1519
1520         if(planes) {
1521                 BLI_pbvh_search_callback(bvh, BLI_pbvh_node_planes_contain_AABB,
1522                                 planes, BLI_pbvh_node_draw, NULL);
1523         }
1524         else {
1525                 BLI_pbvh_search_callback(bvh, NULL, NULL, BLI_pbvh_node_draw, NULL);
1526         }
1527 }
1528
1529 void BLI_pbvh_grids_update(PBVH *bvh, DMGridData **grids, DMGridAdjacency *gridadj, void **gridfaces)
1530 {
1531         bvh->grids= grids;
1532         bvh->gridadj= gridadj;
1533         bvh->gridfaces= gridfaces;
1534 }
1535
1536 float (*BLI_pbvh_get_vertCos(PBVH *pbvh))[3]
1537 {
1538         int a;
1539         float (*vertCos)[3]= NULL;
1540
1541         if (pbvh->verts) {
1542                 float *co;
1543                 MVert *mvert= pbvh->verts;
1544
1545                 vertCos= MEM_callocN(3*pbvh->totvert*sizeof(float), "BLI_pbvh_get_vertCoords");
1546                 co= (float*)vertCos;
1547
1548                 for (a= 0; a<pbvh->totvert; a++, mvert++, co+= 3) {
1549                         copy_v3_v3(co, mvert->co);
1550                 }
1551         }
1552
1553         return vertCos;
1554 }
1555
1556 void BLI_pbvh_apply_vertCos(PBVH *pbvh, float (*vertCos)[3])
1557 {
1558         int a;
1559
1560         if (!pbvh->deformed) {
1561                 if (pbvh->verts) {
1562                         /* if pbvh is not already deformed, verts/faces points to the */
1563                         /* original data and applying new coords to this arrays would lead to */
1564                         /* unneeded deformation -- duplicate verts/faces to avoid this */
1565
1566                         pbvh->verts= MEM_dupallocN(pbvh->verts);
1567                         pbvh->faces= MEM_dupallocN(pbvh->faces);
1568
1569                         pbvh->deformed= 1;
1570                 }
1571         }
1572
1573         if (pbvh->verts) {
1574                 MVert *mvert= pbvh->verts;
1575                 /* copy new verts coords */
1576                 for (a= 0; a < pbvh->totvert; ++a, ++mvert) {
1577                         copy_v3_v3(mvert->co, vertCos[a]);
1578                         mvert->flag |= ME_VERT_PBVH_UPDATE;
1579                 }
1580
1581                 /* coordinates are new -- normals should also be updated */
1582                 mesh_calc_tessface_normals(pbvh->verts, pbvh->totvert, pbvh->faces, pbvh->totprim, NULL);
1583
1584                 for (a= 0; a < pbvh->totnode; ++a)
1585                         BLI_pbvh_node_mark_update(&pbvh->nodes[a]);
1586
1587                 BLI_pbvh_update(pbvh, PBVH_UpdateBB, NULL);
1588                 BLI_pbvh_update(pbvh, PBVH_UpdateOriginalBB, NULL);
1589
1590         }
1591 }
1592
1593 int BLI_pbvh_isDeformed(PBVH *pbvh)
1594 {
1595         return pbvh->deformed;
1596 }
1597 /* Proxies */
1598
1599 PBVHProxyNode* BLI_pbvh_node_add_proxy(PBVH* bvh, PBVHNode* node)
1600 {
1601         int index, totverts;
1602
1603         #pragma omp critical
1604         {
1605
1606                 index = node->proxy_count;
1607
1608                 node->proxy_count++;
1609
1610                 if (node->proxies)
1611                         node->proxies= MEM_reallocN(node->proxies, node->proxy_count*sizeof(PBVHProxyNode));
1612                 else
1613                         node->proxies= MEM_mallocN(sizeof(PBVHProxyNode), "PBVHNodeProxy");
1614
1615                 if (bvh->grids)
1616                         totverts = node->totprim*bvh->gridsize*bvh->gridsize;
1617                 else
1618                         totverts = node->uniq_verts;
1619
1620                 node->proxies[index].co= MEM_callocN(sizeof(float[3])*totverts, "PBVHNodeProxy.co");
1621         }
1622
1623         return node->proxies + index;
1624 }
1625
1626 void BLI_pbvh_node_free_proxies(PBVHNode* node)
1627 {
1628         #pragma omp critical
1629         {
1630                 int p;
1631
1632                 for (p= 0; p < node->proxy_count; p++) {
1633                         MEM_freeN(node->proxies[p].co);
1634                         node->proxies[p].co= 0;
1635                 }
1636
1637                 MEM_freeN(node->proxies);
1638                 node->proxies = 0;
1639
1640                 node->proxy_count= 0;
1641         }
1642 }
1643
1644 void BLI_pbvh_gather_proxies(PBVH* pbvh, PBVHNode*** r_array,  int* r_tot)
1645 {
1646         PBVHNode **array= NULL, **newarray, *node;
1647         int tot= 0, space= 0;
1648         int n;
1649
1650         for (n= 0; n < pbvh->totnode; n++) {
1651                 node = pbvh->nodes + n;
1652
1653                 if(node->proxy_count > 0) {
1654                         if(tot == space) {
1655                                 /* resize array if needed */
1656                                 space= (tot == 0)? 32: space*2;
1657                                 newarray= MEM_callocN(sizeof(PBVHNode)*space, "BLI_pbvh_gather_proxies");
1658
1659                                 if (array) {
1660                                         memcpy(newarray, array, sizeof(PBVHNode)*tot);
1661                                         MEM_freeN(array);
1662                                 }
1663
1664                                 array= newarray;
1665                         }
1666
1667                         array[tot]= node;
1668                         tot++;
1669                 }
1670         }
1671
1672         if(tot == 0 && array) {
1673                 MEM_freeN(array);
1674                 array= NULL;
1675         }
1676
1677         *r_array= array;
1678         *r_tot= tot;
1679 }