7d57dd7d54608b07eec10146200655523646d842
[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->faces,
424                                         node->prim_indices,
425                                         node->totprim,
426                                         node->uniq_verts);
427         }
428
429         node->flag |= PBVH_UpdateDrawBuffers;
430
431         BLI_ghash_free(map, NULL, NULL);
432 }
433
434 static void build_grids_leaf_node(PBVH *bvh, PBVHNode *node)
435 {
436         if(!G.background) {
437                 node->draw_buffers =
438                         GPU_build_grid_buffers(bvh->grids, node->prim_indices,
439                                 node->totprim, bvh->gridsize);
440         }
441         node->flag |= PBVH_UpdateDrawBuffers;
442 }
443
444 /* Recursively build a node in the tree
445
446    vb is the voxel box around all of the primitives contained in
447    this node.
448
449    cb is the bounding box around all the centroids of the primitives
450    contained in this node
451
452    offset and start indicate a range in the array of primitive indices
453 */
454
455 static void build_sub(PBVH *bvh, int node_index, BB *cb, BBC *prim_bbc,
456                    int offset, int count)
457 {
458         int i, axis, end;
459         BB cb_backing;
460
461         /* Decide whether this is a leaf or not */
462         // XXX adapt leaf limit for grids
463         if(count <= bvh->leaf_limit) {
464                 bvh->nodes[node_index].flag |= PBVH_Leaf;
465
466                 bvh->nodes[node_index].prim_indices = bvh->prim_indices + offset;
467                 bvh->nodes[node_index].totprim = count;
468
469                 /* Still need vb for searches */
470                 BB_reset(&bvh->nodes[node_index].vb);
471                 for(i = offset + count - 1; i >= offset; --i) {
472                         BB_expand_with_bb(&bvh->nodes[node_index].vb,
473                                           (BB*)(prim_bbc +
474                                                 bvh->prim_indices[i]));
475                 }
476                 
477                 if(bvh->faces)
478                         build_mesh_leaf_node(bvh, bvh->nodes + node_index);
479                 else
480                         build_grids_leaf_node(bvh, bvh->nodes + node_index);
481                 bvh->nodes[node_index].orig_vb= bvh->nodes[node_index].vb;
482
483                 /* Done with this subtree */
484                 return;
485         }
486         else {
487                 BB_reset(&bvh->nodes[node_index].vb);
488                 bvh->nodes[node_index].children_offset = bvh->totnode;
489                 grow_nodes(bvh, bvh->totnode + 2);
490
491                 if(!cb) {
492                         cb = &cb_backing;
493                         BB_reset(cb);
494                         for(i = offset + count - 1; i >= offset; --i)
495                                 BB_expand(cb, prim_bbc[bvh->prim_indices[i]].bcentroid);
496                 }
497         }
498
499         axis = BB_widest_axis(cb);
500
501         for(i = offset + count - 1; i >= offset; --i) {
502                 BB_expand_with_bb(&bvh->nodes[node_index].vb,
503                                   (BB*)(prim_bbc + bvh->prim_indices[i]));
504         }
505
506         bvh->nodes[node_index].orig_vb= bvh->nodes[node_index].vb;
507
508         end = partition_indices(bvh->prim_indices, offset, offset + count - 1,
509                                 axis,
510                                 (cb->bmax[axis] + cb->bmin[axis]) * 0.5f,
511                                 prim_bbc);
512         check_partitioning(bvh->prim_indices, offset, offset + count - 1,
513                            axis,
514                            (cb->bmax[axis] + cb->bmin[axis]) * 0.5f,
515                            prim_bbc, end);
516
517         build_sub(bvh, bvh->nodes[node_index].children_offset, NULL,
518                   prim_bbc, offset, end - offset);
519         build_sub(bvh, bvh->nodes[node_index].children_offset + 1, NULL,
520                   prim_bbc, end, offset + count - end);
521 }
522
523 static void pbvh_build(PBVH *bvh, BB *cb, BBC *prim_bbc, int totprim)
524 {
525         int i;
526
527         if(totprim != bvh->totprim) {
528                 bvh->totprim = totprim;
529                 if(bvh->nodes) MEM_freeN(bvh->nodes);
530                 if(bvh->prim_indices) MEM_freeN(bvh->prim_indices);
531                 bvh->prim_indices = MEM_callocN(sizeof(int) * totprim,
532                                                 "bvh prim indices");
533                 for(i = 0; i < totprim; ++i)
534                         bvh->prim_indices[i] = i;
535                 bvh->totnode = 0;
536                 if(bvh->node_mem_count < 100) {
537                         bvh->node_mem_count = 100;
538                         bvh->nodes = MEM_callocN(sizeof(PBVHNode) *
539                                                  bvh->node_mem_count,
540                                                  "bvh initial nodes");
541                 }
542         }
543
544         bvh->totnode = 1;
545         build_sub(bvh, 0, cb, prim_bbc, 0, totprim);
546 }
547
548 /* Do a full rebuild with on Mesh data structure */
549 void BLI_pbvh_build_mesh(PBVH *bvh, MFace *faces, MVert *verts, int totface, int totvert)
550 {
551         BBC *prim_bbc = NULL;
552         BB cb;
553         int i, j;
554
555         bvh->faces = faces;
556         bvh->verts = verts;
557         bvh->vert_bitmap = BLI_bitmap_new(totvert);
558         bvh->totvert = totvert;
559         bvh->leaf_limit = LEAF_LIMIT;
560
561         BB_reset(&cb);
562
563         /* For each face, store the AABB and the AABB centroid */
564         prim_bbc = MEM_mallocN(sizeof(BBC) * totface, "prim_bbc");
565
566         for(i = 0; i < totface; ++i) {
567                 MFace *f = faces + i;
568                 const int sides = f->v4 ? 4 : 3;
569                 BBC *bbc = prim_bbc + i;
570
571                 BB_reset((BB*)bbc);
572
573                 for(j = 0; j < sides; ++j)
574                         BB_expand((BB*)bbc, verts[(&f->v1)[j]].co);
575
576                 BBC_update_centroid(bbc);
577
578                 BB_expand(&cb, bbc->bcentroid);
579         }
580
581         if(totface)
582                 pbvh_build(bvh, &cb, prim_bbc, totface);
583
584         MEM_freeN(prim_bbc);
585         MEM_freeN(bvh->vert_bitmap);
586 }
587
588 /* Do a full rebuild with on Grids data structure */
589 void BLI_pbvh_build_grids(PBVH *bvh, DMGridData **grids, DMGridAdjacency *gridadj,
590         int totgrid, int gridsize, void **gridfaces)
591 {
592         BBC *prim_bbc = NULL;
593         BB cb;
594         int i, j;
595
596         bvh->grids= grids;
597         bvh->gridadj= gridadj;
598         bvh->gridfaces= gridfaces;
599         bvh->totgrid= totgrid;
600         bvh->gridsize= gridsize;
601         bvh->leaf_limit = MAX2(LEAF_LIMIT/((gridsize-1)*(gridsize-1)), 1);
602
603         BB_reset(&cb);
604
605         /* For each grid, store the AABB and the AABB centroid */
606         prim_bbc = MEM_mallocN(sizeof(BBC) * totgrid, "prim_bbc");
607
608         for(i = 0; i < totgrid; ++i) {
609                 DMGridData *grid= grids[i];
610                 BBC *bbc = prim_bbc + i;
611
612                 BB_reset((BB*)bbc);
613
614                 for(j = 0; j < gridsize*gridsize; ++j)
615                         BB_expand((BB*)bbc, grid[j].co);
616
617                 BBC_update_centroid(bbc);
618
619                 BB_expand(&cb, bbc->bcentroid);
620         }
621
622         if(totgrid)
623                 pbvh_build(bvh, &cb, prim_bbc, totgrid);
624
625         MEM_freeN(prim_bbc);
626 }
627
628 PBVH *BLI_pbvh_new(void)
629 {
630         PBVH *bvh = MEM_callocN(sizeof(PBVH), "pbvh");
631
632         return bvh;
633 }
634
635 void BLI_pbvh_free(PBVH *bvh)
636 {
637         PBVHNode *node;
638         int i;
639
640         for(i = 0; i < bvh->totnode; ++i) {
641                 node= &bvh->nodes[i];
642
643                 if(node->flag & PBVH_Leaf) {
644                         if(node->draw_buffers)
645                                 GPU_free_buffers(node->draw_buffers);
646                         if(node->vert_indices)
647                                 MEM_freeN(node->vert_indices);
648                         if(node->face_vert_indices)
649                                 MEM_freeN(node->face_vert_indices);
650                 }
651         }
652
653         if (bvh->deformed) {
654                 if (bvh->verts) {
655                         /* if pbvh was deformed, new memory was allocated for verts/faces -- free it */
656
657                         MEM_freeN(bvh->verts);
658                         if(bvh->faces)
659                                 MEM_freeN(bvh->faces);
660                 }
661         }
662
663         if(bvh->nodes)
664                 MEM_freeN(bvh->nodes);
665
666         if(bvh->prim_indices)
667                 MEM_freeN(bvh->prim_indices);
668
669         MEM_freeN(bvh);
670 }
671
672 static void pbvh_iter_begin(PBVHIter *iter, PBVH *bvh, BLI_pbvh_SearchCallback scb, void *search_data)
673 {
674         iter->bvh= bvh;
675         iter->scb= scb;
676         iter->search_data= search_data;
677
678         iter->stack= iter->stackfixed;
679         iter->stackspace= STACK_FIXED_DEPTH;
680
681         iter->stack[0].node= bvh->nodes;
682         iter->stack[0].revisiting= 0;
683         iter->stacksize= 1;
684 }
685
686 static void pbvh_iter_end(PBVHIter *iter)
687 {
688         if(iter->stackspace > STACK_FIXED_DEPTH)
689                 MEM_freeN(iter->stack);
690 }
691
692 static void pbvh_stack_push(PBVHIter *iter, PBVHNode *node, int revisiting)
693 {
694         if(iter->stacksize == iter->stackspace) {
695                 PBVHStack *newstack;
696
697                 iter->stackspace *= 2;
698                 newstack= MEM_callocN(sizeof(PBVHStack)*iter->stackspace, "PBVHStack");
699                 memcpy(newstack, iter->stack, sizeof(PBVHStack)*iter->stacksize);
700
701                 if(iter->stackspace > STACK_FIXED_DEPTH)
702                         MEM_freeN(iter->stack);
703                 iter->stack= newstack;
704         }
705
706         iter->stack[iter->stacksize].node= node;
707         iter->stack[iter->stacksize].revisiting= revisiting;
708         iter->stacksize++;
709 }
710
711 static PBVHNode *pbvh_iter_next(PBVHIter *iter)
712 {
713         PBVHNode *node;
714         int revisiting;
715
716         /* purpose here is to traverse tree, visiting child nodes before their
717            parents, this order is necessary for e.g. computing bounding boxes */
718
719         while(iter->stacksize) {
720                 /* pop node */
721                 iter->stacksize--;
722                 node= iter->stack[iter->stacksize].node;
723
724                 /* on a mesh with no faces this can happen
725                  * can remove this check if we know meshes have at least 1 face */
726                 if(node==NULL)
727                         return NULL;
728
729                 revisiting= iter->stack[iter->stacksize].revisiting;
730
731                 /* revisiting node already checked */
732                 if(revisiting)
733                         return node;
734
735                 if(iter->scb && !iter->scb(node, iter->search_data))
736                         continue; /* don't traverse, outside of search zone */
737
738                 if(node->flag & PBVH_Leaf) {
739                         /* immediately hit leaf node */
740                         return node;
741                 }
742                 else {
743                         /* come back later when children are done */
744                         pbvh_stack_push(iter, node, 1);
745
746                         /* push two child nodes on the stack */
747                         pbvh_stack_push(iter, iter->bvh->nodes+node->children_offset+1, 0);
748                         pbvh_stack_push(iter, iter->bvh->nodes+node->children_offset, 0);
749                 }
750         }
751
752         return NULL;
753 }
754
755 static PBVHNode *pbvh_iter_next_occluded(PBVHIter *iter)
756 {
757         PBVHNode *node;
758
759         while(iter->stacksize) {
760                 /* pop node */
761                 iter->stacksize--;
762                 node= iter->stack[iter->stacksize].node;
763
764                 /* on a mesh with no faces this can happen
765                  * can remove this check if we know meshes have at least 1 face */
766                 if(node==NULL) return NULL;
767
768                 if(iter->scb && !iter->scb(node, iter->search_data)) continue; /* don't traverse, outside of search zone */
769
770                 if(node->flag & PBVH_Leaf) {
771                         /* immediately hit leaf node */
772                         return node;
773                 }
774                 else {
775                         pbvh_stack_push(iter, iter->bvh->nodes+node->children_offset+1, 0);
776                         pbvh_stack_push(iter, iter->bvh->nodes+node->children_offset, 0);
777                 }
778         }
779
780         return NULL;
781 }
782
783 void BLI_pbvh_search_gather(PBVH *bvh,
784         BLI_pbvh_SearchCallback scb, void *search_data,
785         PBVHNode ***r_array, int *r_tot)
786 {
787         PBVHIter iter;
788         PBVHNode **array= NULL, **newarray, *node;
789         int tot= 0, space= 0;
790
791         pbvh_iter_begin(&iter, bvh, scb, search_data);
792
793         while((node=pbvh_iter_next(&iter))) {
794                 if(node->flag & PBVH_Leaf) {
795                         if(tot == space) {
796                                 /* resize array if needed */
797                                 space= (tot == 0)? 32: space*2;
798                                 newarray= MEM_callocN(sizeof(PBVHNode)*space, "PBVHNodeSearch");
799
800                                 if(array) {
801                                         memcpy(newarray, array, sizeof(PBVHNode)*tot);
802                                         MEM_freeN(array);
803                                 }
804
805                                 array= newarray;
806                         }
807
808                         array[tot]= node;
809                         tot++;
810                 }
811         }
812
813         pbvh_iter_end(&iter);
814
815         if(tot == 0 && array) {
816                 MEM_freeN(array);
817                 array= NULL;
818         }
819
820         *r_array= array;
821         *r_tot= tot;
822 }
823
824 void BLI_pbvh_search_callback(PBVH *bvh,
825         BLI_pbvh_SearchCallback scb, void *search_data,
826         BLI_pbvh_HitCallback hcb, void *hit_data)
827 {
828         PBVHIter iter;
829         PBVHNode *node;
830
831         pbvh_iter_begin(&iter, bvh, scb, search_data);
832
833         while((node=pbvh_iter_next(&iter)))
834                 if (node->flag & PBVH_Leaf)
835                         hcb(node, hit_data);
836
837         pbvh_iter_end(&iter);
838 }
839
840 typedef struct node_tree {
841         PBVHNode* data;
842
843         struct node_tree* left;
844         struct node_tree* right;
845 } node_tree;
846
847 static void node_tree_insert(node_tree* tree, node_tree* new_node)
848 {
849         if (new_node->data->tmin < tree->data->tmin) {
850                 if (tree->left) {
851                         node_tree_insert(tree->left, new_node);
852                 }
853                 else {
854                         tree->left = new_node;
855                 }
856         }
857         else {
858                 if (tree->right) {
859                         node_tree_insert(tree->right, new_node);
860                 }
861                 else {
862                         tree->right = new_node;
863                 }
864         }
865 }
866
867 static void traverse_tree(node_tree* tree, BLI_pbvh_HitOccludedCallback hcb, void* hit_data, float* tmin)
868 {
869         if (tree->left) traverse_tree(tree->left, hcb, hit_data, tmin);
870
871         hcb(tree->data, hit_data, tmin);
872
873         if (tree->right) traverse_tree(tree->right, hcb, hit_data, tmin);
874 }
875
876 static void free_tree(node_tree* tree)
877 {
878         if (tree->left) {
879                 free_tree(tree->left);
880                 tree->left = 0;
881         }
882
883         if (tree->right) {
884                 free_tree(tree->right);
885                 tree->right = 0;
886         }
887
888         free(tree);
889 }
890
891 float BLI_pbvh_node_get_tmin(PBVHNode* node)
892 {
893         return node->tmin;
894 }
895
896 static void BLI_pbvh_search_callback_occluded(PBVH *bvh,
897         BLI_pbvh_SearchCallback scb, void *search_data,
898         BLI_pbvh_HitOccludedCallback hcb, void *hit_data)
899 {
900         PBVHIter iter;
901         PBVHNode *node;
902         node_tree *tree = 0;
903
904         pbvh_iter_begin(&iter, bvh, scb, search_data);
905
906         while((node=pbvh_iter_next_occluded(&iter))) {
907                 if(node->flag & PBVH_Leaf) {
908                         node_tree* new_node = malloc(sizeof(node_tree));
909
910                         new_node->data = node;
911
912                         new_node->left  = NULL;
913                         new_node->right = NULL;
914
915                         if (tree) {
916                                 node_tree_insert(tree, new_node);
917                         }
918                         else {
919                                 tree = new_node;
920                         }
921                 }
922         }
923
924         pbvh_iter_end(&iter);
925
926         if (tree) {
927                 float tmin = FLT_MAX;
928                 traverse_tree(tree, hcb, hit_data, &tmin);
929                 free_tree(tree);
930         }
931 }
932
933 static int update_search_cb(PBVHNode *node, void *data_v)
934 {
935         int flag= GET_INT_FROM_POINTER(data_v);
936
937         if(node->flag & PBVH_Leaf)
938                 return (node->flag & flag);
939         
940         return 1;
941 }
942
943 static void pbvh_update_normals(PBVH *bvh, PBVHNode **nodes,
944         int totnode, float (*face_nors)[3])
945 {
946         float (*vnor)[3];
947         int n;
948
949         if(bvh->grids)
950                 return;
951
952         /* could be per node to save some memory, but also means
953            we have to store for each vertex which node it is in */
954         vnor= MEM_callocN(sizeof(float)*3*bvh->totvert, "bvh temp vnors");
955
956         /* subtle assumptions:
957            - We know that for all edited vertices, the nodes with faces
958                  adjacent to these vertices have been marked with PBVH_UpdateNormals.
959                  This is true because if the vertex is inside the brush radius, the
960                  bounding box of it's adjacent faces will be as well.
961            - However this is only true for the vertices that have actually been
962                  edited, not for all vertices in the nodes marked for update, so we
963                  can only update vertices marked with ME_VERT_PBVH_UPDATE.
964         */
965
966         #pragma omp parallel for private(n) schedule(static)
967         for(n = 0; n < totnode; n++) {
968                 PBVHNode *node= nodes[n];
969
970                 if((node->flag & PBVH_UpdateNormals)) {
971                         int i, j, totface, *faces;
972
973                         faces= node->prim_indices;
974                         totface= node->totprim;
975
976                         for(i = 0; i < totface; ++i) {
977                                 MFace *f= bvh->faces + faces[i];
978                                 float fn[3];
979                                 unsigned int *fv = &f->v1;
980                                 int sides= (f->v4)? 4: 3;
981
982                                 if(f->v4)
983                                         normal_quad_v3(fn, bvh->verts[f->v1].co, bvh->verts[f->v2].co,
984                                                                    bvh->verts[f->v3].co, bvh->verts[f->v4].co);
985                                 else
986                                         normal_tri_v3(fn, bvh->verts[f->v1].co, bvh->verts[f->v2].co,
987                                                                   bvh->verts[f->v3].co);
988
989                                 for(j = 0; j < sides; ++j) {
990                                         int v= fv[j];
991
992                                         if(bvh->verts[v].flag & ME_VERT_PBVH_UPDATE) {
993                                                 /* this seems like it could be very slow but profile
994                                                    does not show this, so just leave it for now? */
995                                                 #pragma omp atomic
996                                                 vnor[v][0] += fn[0];
997                                                 #pragma omp atomic
998                                                 vnor[v][1] += fn[1];
999                                                 #pragma omp atomic
1000                                                 vnor[v][2] += fn[2];
1001                                         }
1002                                 }
1003
1004                                 if(face_nors)
1005                                         copy_v3_v3(face_nors[faces[i]], fn);
1006                         }
1007                 }
1008         }
1009
1010         #pragma omp parallel for private(n) schedule(static)
1011         for(n = 0; n < totnode; n++) {
1012                 PBVHNode *node= nodes[n];
1013
1014                 if(node->flag & PBVH_UpdateNormals) {
1015                         int i, *verts, totvert;
1016
1017                         verts= node->vert_indices;
1018                         totvert= node->uniq_verts;
1019
1020                         for(i = 0; i < totvert; ++i) {
1021                                 const int v = verts[i];
1022                                 MVert *mvert= &bvh->verts[v];
1023
1024                                 if(mvert->flag & ME_VERT_PBVH_UPDATE) {
1025                                         float no[3];
1026
1027                                         copy_v3_v3(no, vnor[v]);
1028                                         normalize_v3(no);
1029                                         
1030                                         mvert->no[0] = (short)(no[0]*32767.0f);
1031                                         mvert->no[1] = (short)(no[1]*32767.0f);
1032                                         mvert->no[2] = (short)(no[2]*32767.0f);
1033                                         
1034                                         mvert->flag &= ~ME_VERT_PBVH_UPDATE;
1035                                 }
1036                         }
1037
1038                         node->flag &= ~PBVH_UpdateNormals;
1039                 }
1040         }
1041
1042         MEM_freeN(vnor);
1043 }
1044
1045 static void pbvh_update_BB_redraw(PBVH *bvh, PBVHNode **nodes,
1046         int totnode, int flag)
1047 {
1048         int n;
1049
1050         /* update BB, redraw flag */
1051         #pragma omp parallel for private(n) schedule(static)
1052         for(n = 0; n < totnode; n++) {
1053                 PBVHNode *node= nodes[n];
1054
1055                 if((flag & PBVH_UpdateBB) && (node->flag & PBVH_UpdateBB))
1056                         /* don't clear flag yet, leave it for flushing later */
1057                         update_node_vb(bvh, node);
1058
1059                 if((flag & PBVH_UpdateOriginalBB) && (node->flag & PBVH_UpdateOriginalBB))
1060                         node->orig_vb= node->vb;
1061
1062                 if((flag & PBVH_UpdateRedraw) && (node->flag & PBVH_UpdateRedraw))
1063                         node->flag &= ~PBVH_UpdateRedraw;
1064         }
1065 }
1066
1067 static void pbvh_update_draw_buffers(PBVH *bvh, PBVHNode **nodes, int totnode, int smooth)
1068 {
1069         PBVHNode *node;
1070         int n;
1071
1072         /* can't be done in parallel with OpenGL */
1073         for(n = 0; n < totnode; n++) {
1074                 node= nodes[n];
1075
1076                 if(node->flag & PBVH_UpdateDrawBuffers) {
1077                         if(bvh->grids) {
1078                                 GPU_update_grid_buffers(node->draw_buffers,
1079                                                    bvh->grids,
1080                                                    node->prim_indices,
1081                                                    node->totprim,
1082                                                    bvh->gridsize,
1083                                                    smooth);
1084                         }
1085                         else {
1086                                 GPU_update_mesh_buffers(node->draw_buffers,
1087                                                    bvh->verts,
1088                                                    node->vert_indices,
1089                                                    node->uniq_verts +
1090                                                    node->face_verts,
1091                                    smooth);
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_normals_tessface(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 }
1686
1687 void pbvh_vertex_iter_init(PBVH *bvh, PBVHNode *node,
1688                                                    PBVHVertexIter *vi, int mode)
1689 {
1690         struct DMGridData **grids;
1691         struct MVert *verts;
1692         int *grid_indices, *vert_indices;
1693         int totgrid, gridsize, uniq_verts, totvert;
1694         
1695         vi->grid= 0;
1696         vi->no= 0;
1697         vi->fno= 0;
1698         vi->mvert= 0;
1699         
1700         BLI_pbvh_node_get_grids(bvh, node, &grid_indices, &totgrid, NULL, &gridsize, &grids, NULL);
1701         BLI_pbvh_node_num_verts(bvh, node, &uniq_verts, &totvert);
1702         BLI_pbvh_node_get_verts(bvh, node, &vert_indices, &verts);
1703         
1704         vi->grids= grids;
1705         vi->grid_indices= grid_indices;
1706         vi->totgrid= (grids)? totgrid: 1;
1707         vi->gridsize= gridsize;
1708         
1709         if(mode == PBVH_ITER_ALL)
1710                 vi->totvert = totvert;
1711         else
1712                 vi->totvert= uniq_verts;
1713         vi->vert_indices= vert_indices;
1714         vi->mverts= verts;
1715 }