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