Made minor revisions (no functional changes).
[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                         MEM_freeN(bvh->faces);
660                 }
661         }
662
663         MEM_freeN(bvh->nodes);
664         MEM_freeN(bvh->prim_indices);
665         MEM_freeN(bvh);
666 }
667
668 static void pbvh_iter_begin(PBVHIter *iter, PBVH *bvh, BLI_pbvh_SearchCallback scb, void *search_data)
669 {
670         iter->bvh= bvh;
671         iter->scb= scb;
672         iter->search_data= search_data;
673
674         iter->stack= iter->stackfixed;
675         iter->stackspace= STACK_FIXED_DEPTH;
676
677         iter->stack[0].node= bvh->nodes;
678         iter->stack[0].revisiting= 0;
679         iter->stacksize= 1;
680 }
681
682 static void pbvh_iter_end(PBVHIter *iter)
683 {
684         if(iter->stackspace > STACK_FIXED_DEPTH)
685                 MEM_freeN(iter->stack);
686 }
687
688 static void pbvh_stack_push(PBVHIter *iter, PBVHNode *node, int revisiting)
689 {
690         if(iter->stacksize == iter->stackspace) {
691                 PBVHStack *newstack;
692
693                 iter->stackspace *= 2;
694                 newstack= MEM_callocN(sizeof(PBVHStack)*iter->stackspace, "PBVHStack");
695                 memcpy(newstack, iter->stack, sizeof(PBVHStack)*iter->stacksize);
696
697                 if(iter->stackspace > STACK_FIXED_DEPTH)
698                         MEM_freeN(iter->stack);
699                 iter->stack= newstack;
700         }
701
702         iter->stack[iter->stacksize].node= node;
703         iter->stack[iter->stacksize].revisiting= revisiting;
704         iter->stacksize++;
705 }
706
707 static PBVHNode *pbvh_iter_next(PBVHIter *iter)
708 {
709         PBVHNode *node;
710         int revisiting;
711
712         /* purpose here is to traverse tree, visiting child nodes before their
713            parents, this order is necessary for e.g. computing bounding boxes */
714
715         while(iter->stacksize) {
716                 /* pop node */
717                 iter->stacksize--;
718                 node= iter->stack[iter->stacksize].node;
719
720                 /* on a mesh with no faces this can happen
721                  * can remove this check if we know meshes have at least 1 face */
722                 if(node==NULL)
723                         return NULL;
724
725                 revisiting= iter->stack[iter->stacksize].revisiting;
726
727                 /* revisiting node already checked */
728                 if(revisiting)
729                         return node;
730
731                 if(iter->scb && !iter->scb(node, iter->search_data))
732                         continue; /* don't traverse, outside of search zone */
733
734                 if(node->flag & PBVH_Leaf) {
735                         /* immediately hit leaf node */
736                         return node;
737                 }
738                 else {
739                         /* come back later when children are done */
740                         pbvh_stack_push(iter, node, 1);
741
742                         /* push two child nodes on the stack */
743                         pbvh_stack_push(iter, iter->bvh->nodes+node->children_offset+1, 0);
744                         pbvh_stack_push(iter, iter->bvh->nodes+node->children_offset, 0);
745                 }
746         }
747
748         return NULL;
749 }
750
751 static PBVHNode *pbvh_iter_next_occluded(PBVHIter *iter)
752 {
753         PBVHNode *node;
754
755         while(iter->stacksize) {
756                 /* pop node */
757                 iter->stacksize--;
758                 node= iter->stack[iter->stacksize].node;
759
760                 /* on a mesh with no faces this can happen
761                  * can remove this check if we know meshes have at least 1 face */
762                 if(node==NULL) return NULL;
763
764                 if(iter->scb && !iter->scb(node, iter->search_data)) continue; /* don't traverse, outside of search zone */
765
766                 if(node->flag & PBVH_Leaf) {
767                         /* immediately hit leaf node */
768                         return node;
769                 }
770                 else {
771                         pbvh_stack_push(iter, iter->bvh->nodes+node->children_offset+1, 0);
772                         pbvh_stack_push(iter, iter->bvh->nodes+node->children_offset, 0);
773                 }
774         }
775
776         return NULL;
777 }
778
779 void BLI_pbvh_search_gather(PBVH *bvh,
780         BLI_pbvh_SearchCallback scb, void *search_data,
781         PBVHNode ***r_array, int *r_tot)
782 {
783         PBVHIter iter;
784         PBVHNode **array= NULL, **newarray, *node;
785         int tot= 0, space= 0;
786
787         pbvh_iter_begin(&iter, bvh, scb, search_data);
788
789         while((node=pbvh_iter_next(&iter))) {
790                 if(node->flag & PBVH_Leaf) {
791                         if(tot == space) {
792                                 /* resize array if needed */
793                                 space= (tot == 0)? 32: space*2;
794                                 newarray= MEM_callocN(sizeof(PBVHNode)*space, "PBVHNodeSearch");
795
796                                 if(array) {
797                                         memcpy(newarray, array, sizeof(PBVHNode)*tot);
798                                         MEM_freeN(array);
799                                 }
800
801                                 array= newarray;
802                         }
803
804                         array[tot]= node;
805                         tot++;
806                 }
807         }
808
809         pbvh_iter_end(&iter);
810
811         if(tot == 0 && array) {
812                 MEM_freeN(array);
813                 array= NULL;
814         }
815
816         *r_array= array;
817         *r_tot= tot;
818 }
819
820 void BLI_pbvh_search_callback(PBVH *bvh,
821         BLI_pbvh_SearchCallback scb, void *search_data,
822         BLI_pbvh_HitCallback hcb, void *hit_data)
823 {
824         PBVHIter iter;
825         PBVHNode *node;
826
827         pbvh_iter_begin(&iter, bvh, scb, search_data);
828
829         while((node=pbvh_iter_next(&iter)))
830                 if (node->flag & PBVH_Leaf)
831                         hcb(node, hit_data);
832
833         pbvh_iter_end(&iter);
834 }
835
836 typedef struct node_tree {
837         PBVHNode* data;
838
839         struct node_tree* left;
840         struct node_tree* right;
841 } node_tree;
842
843 static void node_tree_insert(node_tree* tree, node_tree* new_node)
844 {
845         if (new_node->data->tmin < tree->data->tmin) {
846                 if (tree->left) {
847                         node_tree_insert(tree->left, new_node);
848                 }
849                 else {
850                         tree->left = new_node;
851                 }
852         }
853         else {
854                 if (tree->right) {
855                         node_tree_insert(tree->right, new_node);
856                 }
857                 else {
858                         tree->right = new_node;
859                 }
860         }
861 }
862
863 static void traverse_tree(node_tree* tree, BLI_pbvh_HitOccludedCallback hcb, void* hit_data, float* tmin)
864 {
865         if (tree->left) traverse_tree(tree->left, hcb, hit_data, tmin);
866
867         hcb(tree->data, hit_data, tmin);
868
869         if (tree->right) traverse_tree(tree->right, hcb, hit_data, tmin);
870 }
871
872 static void free_tree(node_tree* tree)
873 {
874         if (tree->left) {
875                 free_tree(tree->left);
876                 tree->left = 0;
877         }
878
879         if (tree->right) {
880                 free_tree(tree->right);
881                 tree->right = 0;
882         }
883
884         free(tree);
885 }
886
887 float BLI_pbvh_node_get_tmin(PBVHNode* node)
888 {
889         return node->tmin;
890 }
891
892 static void BLI_pbvh_search_callback_occluded(PBVH *bvh,
893         BLI_pbvh_SearchCallback scb, void *search_data,
894         BLI_pbvh_HitOccludedCallback hcb, void *hit_data)
895 {
896         PBVHIter iter;
897         PBVHNode *node;
898         node_tree *tree = 0;
899
900         pbvh_iter_begin(&iter, bvh, scb, search_data);
901
902         while((node=pbvh_iter_next_occluded(&iter))) {
903                 if(node->flag & PBVH_Leaf) {
904                         node_tree* new_node = malloc(sizeof(node_tree));
905
906                         new_node->data = node;
907
908                         new_node->left  = NULL;
909                         new_node->right = NULL;
910
911                         if (tree) {
912                                 node_tree_insert(tree, new_node);
913                         }
914                         else {
915                                 tree = new_node;
916                         }
917                 }
918         }
919
920         pbvh_iter_end(&iter);
921
922         if (tree) {
923                 float tmin = FLT_MAX;
924                 traverse_tree(tree, hcb, hit_data, &tmin);
925                 free_tree(tree);
926         }
927 }
928
929 static int update_search_cb(PBVHNode *node, void *data_v)
930 {
931         int flag= GET_INT_FROM_POINTER(data_v);
932
933         if(node->flag & PBVH_Leaf)
934                 return (node->flag & flag);
935         
936         return 1;
937 }
938
939 static void pbvh_update_normals(PBVH *bvh, PBVHNode **nodes,
940         int totnode, float (*face_nors)[3])
941 {
942         float (*vnor)[3];
943         int n;
944
945         if(bvh->grids)
946                 return;
947
948         /* could be per node to save some memory, but also means
949            we have to store for each vertex which node it is in */
950         vnor= MEM_callocN(sizeof(float)*3*bvh->totvert, "bvh temp vnors");
951
952         /* subtle assumptions:
953            - We know that for all edited vertices, the nodes with faces
954                  adjacent to these vertices have been marked with PBVH_UpdateNormals.
955                  This is true because if the vertex is inside the brush radius, the
956                  bounding box of it's adjacent faces will be as well.
957            - However this is only true for the vertices that have actually been
958                  edited, not for all vertices in the nodes marked for update, so we
959                  can only update vertices marked with ME_VERT_PBVH_UPDATE.
960         */
961
962         #pragma omp parallel for private(n) schedule(static)
963         for(n = 0; n < totnode; n++) {
964                 PBVHNode *node= nodes[n];
965
966                 if((node->flag & PBVH_UpdateNormals)) {
967                         int i, j, totface, *faces;
968
969                         faces= node->prim_indices;
970                         totface= node->totprim;
971
972                         for(i = 0; i < totface; ++i) {
973                                 MFace *f= bvh->faces + faces[i];
974                                 float fn[3];
975                                 unsigned int *fv = &f->v1;
976                                 int sides= (f->v4)? 4: 3;
977
978                                 if(f->v4)
979                                         normal_quad_v3(fn, bvh->verts[f->v1].co, bvh->verts[f->v2].co,
980                                                                    bvh->verts[f->v3].co, bvh->verts[f->v4].co);
981                                 else
982                                         normal_tri_v3(fn, bvh->verts[f->v1].co, bvh->verts[f->v2].co,
983                                                                   bvh->verts[f->v3].co);
984
985                                 for(j = 0; j < sides; ++j) {
986                                         int v= fv[j];
987
988                                         if(bvh->verts[v].flag & ME_VERT_PBVH_UPDATE) {
989                                                 /* this seems like it could be very slow but profile
990                                                    does not show this, so just leave it for now? */
991                                                 #pragma omp atomic
992                                                 vnor[v][0] += fn[0];
993                                                 #pragma omp atomic
994                                                 vnor[v][1] += fn[1];
995                                                 #pragma omp atomic
996                                                 vnor[v][2] += fn[2];
997                                         }
998                                 }
999
1000                                 if(face_nors)
1001                                         copy_v3_v3(face_nors[faces[i]], fn);
1002                         }
1003                 }
1004         }
1005
1006         #pragma omp parallel for private(n) schedule(static)
1007         for(n = 0; n < totnode; n++) {
1008                 PBVHNode *node= nodes[n];
1009
1010                 if(node->flag & PBVH_UpdateNormals) {
1011                         int i, *verts, totvert;
1012
1013                         verts= node->vert_indices;
1014                         totvert= node->uniq_verts;
1015
1016                         for(i = 0; i < totvert; ++i) {
1017                                 const int v = verts[i];
1018                                 MVert *mvert= &bvh->verts[v];
1019
1020                                 if(mvert->flag & ME_VERT_PBVH_UPDATE) {
1021                                         float no[3];
1022
1023                                         copy_v3_v3(no, vnor[v]);
1024                                         normalize_v3(no);
1025                                         
1026                                         mvert->no[0] = (short)(no[0]*32767.0f);
1027                                         mvert->no[1] = (short)(no[1]*32767.0f);
1028                                         mvert->no[2] = (short)(no[2]*32767.0f);
1029                                         
1030                                         mvert->flag &= ~ME_VERT_PBVH_UPDATE;
1031                                 }
1032                         }
1033
1034                         node->flag &= ~PBVH_UpdateNormals;
1035                 }
1036         }
1037
1038         MEM_freeN(vnor);
1039 }
1040
1041 static void pbvh_update_BB_redraw(PBVH *bvh, PBVHNode **nodes,
1042         int totnode, int flag)
1043 {
1044         int n;
1045
1046         /* update BB, redraw flag */
1047         #pragma omp parallel for private(n) schedule(static)
1048         for(n = 0; n < totnode; n++) {
1049                 PBVHNode *node= nodes[n];
1050
1051                 if((flag & PBVH_UpdateBB) && (node->flag & PBVH_UpdateBB))
1052                         /* don't clear flag yet, leave it for flushing later */
1053                         update_node_vb(bvh, node);
1054
1055                 if((flag & PBVH_UpdateOriginalBB) && (node->flag & PBVH_UpdateOriginalBB))
1056                         node->orig_vb= node->vb;
1057
1058                 if((flag & PBVH_UpdateRedraw) && (node->flag & PBVH_UpdateRedraw))
1059                         node->flag &= ~PBVH_UpdateRedraw;
1060         }
1061 }
1062
1063 static void pbvh_update_draw_buffers(PBVH *bvh, PBVHNode **nodes, int totnode, int smooth)
1064 {
1065         PBVHNode *node;
1066         int n;
1067
1068         /* can't be done in parallel with OpenGL */
1069         for(n = 0; n < totnode; n++) {
1070                 node= nodes[n];
1071
1072                 if(node->flag & PBVH_UpdateDrawBuffers) {
1073                         if(bvh->grids) {
1074                                 GPU_update_grid_buffers(node->draw_buffers,
1075                                                    bvh->grids,
1076                                                    node->prim_indices,
1077                                                    node->totprim,
1078                                                    bvh->gridsize,
1079                                                    smooth);
1080                         }
1081                         else {
1082                                 GPU_update_mesh_buffers(node->draw_buffers,
1083                                                    bvh->verts,
1084                                                    node->vert_indices,
1085                                                    node->uniq_verts +
1086                                                    node->face_verts);
1087                         }
1088
1089                         node->flag &= ~PBVH_UpdateDrawBuffers;
1090                 }
1091         }
1092 }
1093
1094 static int pbvh_flush_bb(PBVH *bvh, PBVHNode *node, int flag)
1095 {
1096         int update= 0;
1097
1098         /* difficult to multithread well, we just do single threaded recursive */
1099         if(node->flag & PBVH_Leaf) {
1100                 if(flag & PBVH_UpdateBB) {
1101                         update |= (node->flag & PBVH_UpdateBB);
1102                         node->flag &= ~PBVH_UpdateBB;
1103                 }
1104
1105                 if(flag & PBVH_UpdateOriginalBB) {
1106                         update |= (node->flag & PBVH_UpdateOriginalBB);
1107                         node->flag &= ~PBVH_UpdateOriginalBB;
1108                 }
1109
1110                 return update;
1111         }
1112         else {
1113                 update |= pbvh_flush_bb(bvh, bvh->nodes + node->children_offset, flag);
1114                 update |= pbvh_flush_bb(bvh, bvh->nodes + node->children_offset + 1, flag);
1115
1116                 if(update & PBVH_UpdateBB)
1117                         update_node_vb(bvh, node);
1118                 if(update & PBVH_UpdateOriginalBB)
1119                         node->orig_vb= node->vb;
1120         }
1121
1122         return update;
1123 }
1124
1125 void BLI_pbvh_update(PBVH *bvh, int flag, float (*face_nors)[3])
1126 {
1127         PBVHNode **nodes;
1128         int totnode;
1129
1130         BLI_pbvh_search_gather(bvh, update_search_cb, SET_INT_IN_POINTER(flag),
1131                 &nodes, &totnode);
1132
1133         if(flag & PBVH_UpdateNormals)
1134                 pbvh_update_normals(bvh, nodes, totnode, face_nors);
1135
1136         if(flag & (PBVH_UpdateBB|PBVH_UpdateOriginalBB|PBVH_UpdateRedraw))
1137                 pbvh_update_BB_redraw(bvh, nodes, totnode, flag);
1138
1139         if(flag & (PBVH_UpdateBB|PBVH_UpdateOriginalBB))
1140                 pbvh_flush_bb(bvh, bvh->nodes, flag);
1141
1142         if(nodes) MEM_freeN(nodes);
1143 }
1144
1145 void BLI_pbvh_redraw_BB(PBVH *bvh, float bb_min[3], float bb_max[3])
1146 {
1147         PBVHIter iter;
1148         PBVHNode *node;
1149         BB bb;
1150
1151         BB_reset(&bb);
1152
1153         pbvh_iter_begin(&iter, bvh, NULL, NULL);
1154
1155         while((node=pbvh_iter_next(&iter)))
1156                 if(node->flag & PBVH_UpdateRedraw)
1157                         BB_expand_with_bb(&bb, &node->vb);
1158
1159         pbvh_iter_end(&iter);
1160
1161         copy_v3_v3(bb_min, bb.bmin);
1162         copy_v3_v3(bb_max, bb.bmax);
1163 }
1164
1165 void BLI_pbvh_get_grid_updates(PBVH *bvh, int clear, void ***gridfaces, int *totface)
1166 {
1167         PBVHIter iter;
1168         PBVHNode *node;
1169         GHashIterator *hiter;
1170         GHash *map;
1171         void *face, **faces;
1172         unsigned i;
1173         int tot;
1174
1175         map = BLI_ghash_new(BLI_ghashutil_ptrhash, BLI_ghashutil_ptrcmp, "pbvh_get_grid_updates gh");
1176
1177         pbvh_iter_begin(&iter, bvh, NULL, NULL);
1178
1179         while((node=pbvh_iter_next(&iter))) {
1180                 if(node->flag & PBVH_UpdateNormals) {
1181                         for(i = 0; i < node->totprim; ++i) {
1182                                 face= bvh->gridfaces[node->prim_indices[i]];
1183                                 if(!BLI_ghash_lookup(map, face))
1184                                         BLI_ghash_insert(map, face, face);
1185                         }
1186
1187                         if(clear)
1188                                 node->flag &= ~PBVH_UpdateNormals;
1189                 }
1190         }
1191
1192         pbvh_iter_end(&iter);
1193         
1194         tot= BLI_ghash_size(map);
1195         if(tot == 0) {
1196                 *totface= 0;
1197                 *gridfaces= NULL;
1198                 BLI_ghash_free(map, NULL, NULL);
1199                 return;
1200         }
1201
1202         faces= MEM_callocN(sizeof(void*)*tot, "PBVH Grid Faces");
1203
1204         for(hiter = BLI_ghashIterator_new(map), i = 0;
1205                 !BLI_ghashIterator_isDone(hiter);
1206                 BLI_ghashIterator_step(hiter), ++i)
1207                 faces[i]= BLI_ghashIterator_getKey(hiter);
1208
1209         BLI_ghashIterator_free(hiter);
1210
1211         BLI_ghash_free(map, NULL, NULL);
1212
1213         *totface= tot;
1214         *gridfaces= faces;
1215 }
1216
1217 /***************************** Node Access ***********************************/
1218
1219 void BLI_pbvh_node_mark_update(PBVHNode *node)
1220 {
1221         node->flag |= PBVH_UpdateNormals|PBVH_UpdateBB|PBVH_UpdateOriginalBB|PBVH_UpdateDrawBuffers|PBVH_UpdateRedraw;
1222 }
1223
1224 void BLI_pbvh_node_get_verts(PBVH *bvh, PBVHNode *node, int **vert_indices, MVert **verts)
1225 {
1226         if(vert_indices) *vert_indices= node->vert_indices;
1227         if(verts) *verts= bvh->verts;
1228 }
1229
1230 void BLI_pbvh_node_num_verts(PBVH *bvh, PBVHNode *node, int *uniquevert, int *totvert)
1231 {
1232         if(bvh->grids) {
1233                 const int tot= node->totprim*bvh->gridsize*bvh->gridsize;
1234                 if(totvert) *totvert= tot;
1235                 if(uniquevert) *uniquevert= tot;
1236         }
1237         else {
1238                 if(totvert) *totvert= node->uniq_verts + node->face_verts;
1239                 if(uniquevert) *uniquevert= node->uniq_verts;
1240         }
1241 }
1242
1243 void BLI_pbvh_node_get_grids(PBVH *bvh, PBVHNode *node, int **grid_indices, int *totgrid, int *maxgrid, int *gridsize, DMGridData ***griddata, DMGridAdjacency **gridadj)
1244 {
1245         if(bvh->grids) {
1246                 if(grid_indices) *grid_indices= node->prim_indices;
1247                 if(totgrid) *totgrid= node->totprim;
1248                 if(maxgrid) *maxgrid= bvh->totgrid;
1249                 if(gridsize) *gridsize= bvh->gridsize;
1250                 if(griddata) *griddata= bvh->grids;
1251                 if(gridadj) *gridadj= bvh->gridadj;
1252         }
1253         else {
1254                 if(grid_indices) *grid_indices= NULL;
1255                 if(totgrid) *totgrid= 0;
1256                 if(maxgrid) *maxgrid= 0;
1257                 if(gridsize) *gridsize= 0;
1258                 if(griddata) *griddata= NULL;
1259                 if(gridadj) *gridadj= NULL;
1260         }
1261 }
1262
1263 void BLI_pbvh_node_get_BB(PBVHNode *node, float bb_min[3], float bb_max[3])
1264 {
1265         copy_v3_v3(bb_min, node->vb.bmin);
1266         copy_v3_v3(bb_max, node->vb.bmax);
1267 }
1268
1269 void BLI_pbvh_node_get_original_BB(PBVHNode *node, float bb_min[3], float bb_max[3])
1270 {
1271         copy_v3_v3(bb_min, node->orig_vb.bmin);
1272         copy_v3_v3(bb_max, node->orig_vb.bmax);
1273 }
1274
1275 void BLI_pbvh_node_get_proxies(PBVHNode* node, PBVHProxyNode** proxies, int* proxy_count)
1276 {
1277         if (node->proxy_count > 0) {
1278                 if (proxies) *proxies = node->proxies;
1279                 if (proxy_count) *proxy_count = node->proxy_count;
1280         }
1281         else {
1282                 if (proxies) *proxies = 0;
1283                 if (proxy_count) *proxy_count = 0;
1284         }
1285 }
1286
1287 /********************************* Raycast ***********************************/
1288
1289 typedef struct {
1290         /* Ray */
1291         float start[3];
1292         int sign[3];
1293         float inv_dir[3];
1294         int original;
1295 } RaycastData;
1296
1297 /* Adapted from here: http://www.gamedev.net/community/forums/topic.asp?topic_id=459973 */
1298 static int ray_aabb_intersect(PBVHNode *node, void *data_v)
1299 {
1300         RaycastData *ray = data_v;
1301         float bbox[2][3];
1302         float tmin, tmax, tymin, tymax, tzmin, tzmax;
1303
1304         if(ray->original)
1305                 BLI_pbvh_node_get_original_BB(node, bbox[0], bbox[1]);
1306         else
1307                 BLI_pbvh_node_get_BB(node, bbox[0], bbox[1]);
1308
1309         tmin = (bbox[ray->sign[0]][0] - ray->start[0]) * ray->inv_dir[0];
1310         tmax = (bbox[1-ray->sign[0]][0] - ray->start[0]) * ray->inv_dir[0];
1311
1312         tymin = (bbox[ray->sign[1]][1] - ray->start[1]) * ray->inv_dir[1];
1313         tymax = (bbox[1-ray->sign[1]][1] - ray->start[1]) * ray->inv_dir[1];
1314
1315         if((tmin > tymax) || (tymin > tmax))
1316                 return 0;
1317
1318         if(tymin > tmin)
1319                 tmin = tymin;
1320
1321         if(tymax < tmax)
1322                 tmax = tymax;
1323
1324         tzmin = (bbox[ray->sign[2]][2] - ray->start[2]) * ray->inv_dir[2];
1325         tzmax = (bbox[1-ray->sign[2]][2] - ray->start[2]) * ray->inv_dir[2];
1326
1327         if((tmin > tzmax) || (tzmin > tmax))
1328                 return 0;
1329
1330         if(tzmin > tmin)
1331                 tmin = tzmin;
1332
1333         // XXX jwilkins: tmax does not need to be updated since we don't use it
1334         // keeping this here for future reference
1335         //if(tzmax < tmax) tmax = tzmax; 
1336
1337         node->tmin = tmin;
1338
1339         return 1;
1340 }
1341
1342 void BLI_pbvh_raycast(PBVH *bvh, BLI_pbvh_HitOccludedCallback cb, void *data,
1343                           float ray_start[3], float ray_normal[3], int original)
1344 {
1345         RaycastData rcd;
1346
1347         copy_v3_v3(rcd.start, ray_start);
1348         rcd.inv_dir[0] = 1.0f / ray_normal[0];
1349         rcd.inv_dir[1] = 1.0f / ray_normal[1];
1350         rcd.inv_dir[2] = 1.0f / ray_normal[2];
1351         rcd.sign[0] = rcd.inv_dir[0] < 0;
1352         rcd.sign[1] = rcd.inv_dir[1] < 0;
1353         rcd.sign[2] = rcd.inv_dir[2] < 0;
1354         rcd.original = original;
1355
1356         BLI_pbvh_search_callback_occluded(bvh, ray_aabb_intersect, &rcd, cb, data);
1357 }
1358
1359 static int ray_face_intersection(float ray_start[3], float ray_normal[3],
1360                                  float *t0, float *t1, float *t2, float *t3,
1361                                  float *fdist)
1362 {
1363         float dist;
1364
1365         if ((isect_ray_tri_epsilon_v3(ray_start, ray_normal, t0, t1, t2, &dist, NULL, 0.1f) && dist < *fdist) ||
1366            (t3 && isect_ray_tri_epsilon_v3(ray_start, ray_normal, t0, t2, t3, &dist, NULL, 0.1f) && dist < *fdist))
1367         {
1368                 *fdist = dist;
1369                 return 1;
1370         }
1371         else {
1372                 return 0;
1373         }
1374 }
1375
1376 int BLI_pbvh_node_raycast(PBVH *bvh, PBVHNode *node, float (*origco)[3],
1377         float ray_start[3], float ray_normal[3], float *dist)
1378 {
1379         int hit= 0;
1380
1381         if(bvh->faces) {
1382                 MVert *vert = bvh->verts;
1383                 int *faces= node->prim_indices;
1384                 int totface= node->totprim;
1385                 int i;
1386
1387                 for(i = 0; i < totface; ++i) {
1388                         MFace *f = bvh->faces + faces[i];
1389                         int *face_verts = node->face_vert_indices[i];
1390
1391                         if(origco) {
1392                                 /* intersect with backuped original coordinates */
1393                                 hit |= ray_face_intersection(ray_start, ray_normal,
1394                                                          origco[face_verts[0]],
1395                                                          origco[face_verts[1]],
1396                                                          origco[face_verts[2]],
1397                                                          f->v4? origco[face_verts[3]]: NULL,
1398                                                          dist);
1399                         }
1400                         else {
1401                                 /* intersect with current coordinates */
1402                                 hit |= ray_face_intersection(ray_start, ray_normal,
1403                                                          vert[f->v1].co,
1404                                                          vert[f->v2].co,
1405                                                          vert[f->v3].co,
1406                                                          f->v4 ? vert[f->v4].co : NULL,
1407                                                          dist);
1408                         }
1409                 }
1410         }
1411         else {
1412                 int totgrid= node->totprim;
1413                 int gridsize= bvh->gridsize;
1414                 int i, x, y;
1415
1416                 for(i = 0; i < totgrid; ++i) {
1417                         DMGridData *grid= bvh->grids[node->prim_indices[i]];
1418                         if (!grid)
1419                                 continue;
1420
1421                         for(y = 0; y < gridsize-1; ++y) {
1422                                 for(x = 0; x < gridsize-1; ++x) {
1423                                         if(origco) {
1424                                                 hit |= ray_face_intersection(ray_start, ray_normal,
1425                                                                          origco[y*gridsize + x],
1426                                                                          origco[y*gridsize + x+1],
1427                                                                          origco[(y+1)*gridsize + x+1],
1428                                                                          origco[(y+1)*gridsize + x],
1429                                                                          dist);
1430                                         }
1431                                         else {
1432                                                 hit |= ray_face_intersection(ray_start, ray_normal,
1433                                                                          grid[y*gridsize + x].co,
1434                                                                          grid[y*gridsize + x+1].co,
1435                                                                          grid[(y+1)*gridsize + x+1].co,
1436                                                                          grid[(y+1)*gridsize + x].co,
1437                                                                          dist);
1438                                         }
1439                                 }
1440                         }
1441
1442                         if(origco)
1443                                 origco += gridsize*gridsize;
1444                 }
1445         }
1446
1447         return hit;
1448 }
1449
1450 //#include <GL/glew.h>
1451
1452 void BLI_pbvh_node_draw(PBVHNode *node, void *UNUSED(data))
1453 {
1454 #if 0
1455         /* XXX: Just some quick code to show leaf nodes in different colors */
1456         float col[3]; int i;
1457
1458         if(0) { //is_partial) {
1459                 col[0] = (rand() / (float)RAND_MAX); col[1] = col[2] = 0.6;
1460         }
1461         else {
1462                 srand((long long)node);
1463                 for(i = 0; i < 3; ++i)
1464                         col[i] = (rand() / (float)RAND_MAX) * 0.3 + 0.7;
1465         }
1466         glMaterialfv(GL_FRONT_AND_BACK, GL_DIFFUSE, col);
1467
1468         glColor3f(1, 0, 0);
1469 #endif
1470         GPU_draw_buffers(node->draw_buffers);
1471 }
1472
1473 /* Adapted from:
1474    http://www.gamedev.net/community/forums/topic.asp?topic_id=512123
1475    Returns true if the AABB is at least partially within the frustum
1476    (ok, not a real frustum), false otherwise.
1477 */
1478 int BLI_pbvh_node_planes_contain_AABB(PBVHNode *node, void *data)
1479 {
1480         float (*planes)[4] = data;
1481         int i, axis;
1482         float vmin[3] /*, vmax[3]*/, bb_min[3], bb_max[3];
1483
1484         BLI_pbvh_node_get_BB(node, bb_min, bb_max);
1485
1486         for(i = 0; i < 4; ++i) { 
1487                 for(axis = 0; axis < 3; ++axis) {
1488                         if(planes[i][axis] > 0) { 
1489                                 vmin[axis] = bb_min[axis];
1490                                 /*vmax[axis] = bb_max[axis];*/ /*UNUSED*/
1491                         }
1492                         else {
1493                                 vmin[axis] = bb_max[axis];
1494                                 /*vmax[axis] = bb_min[axis];*/ /*UNUSED*/
1495                         }
1496                 }
1497                 
1498                 if(dot_v3v3(planes[i], vmin) + planes[i][3] > 0)
1499                         return 0;
1500         } 
1501
1502         return 1;
1503 }
1504
1505 void BLI_pbvh_draw(PBVH *bvh, float (*planes)[4], float (*face_nors)[3], int smooth)
1506 {
1507         PBVHNode **nodes;
1508         int totnode;
1509
1510         BLI_pbvh_search_gather(bvh, update_search_cb, SET_INT_IN_POINTER(PBVH_UpdateNormals|PBVH_UpdateDrawBuffers),
1511                 &nodes, &totnode);
1512
1513         pbvh_update_normals(bvh, nodes, totnode, face_nors);
1514         pbvh_update_draw_buffers(bvh, nodes, totnode, smooth);
1515
1516         if(nodes) MEM_freeN(nodes);
1517
1518         if(planes) {
1519                 BLI_pbvh_search_callback(bvh, BLI_pbvh_node_planes_contain_AABB,
1520                                 planes, BLI_pbvh_node_draw, NULL);
1521         }
1522         else {
1523                 BLI_pbvh_search_callback(bvh, NULL, NULL, BLI_pbvh_node_draw, NULL);
1524         }
1525 }
1526
1527 void BLI_pbvh_grids_update(PBVH *bvh, DMGridData **grids, DMGridAdjacency *gridadj, void **gridfaces)
1528 {
1529         bvh->grids= grids;
1530         bvh->gridadj= gridadj;
1531         bvh->gridfaces= gridfaces;
1532 }
1533
1534 float (*BLI_pbvh_get_vertCos(PBVH *pbvh))[3]
1535 {
1536         int a;
1537         float (*vertCos)[3]= NULL;
1538
1539         if (pbvh->verts) {
1540                 float *co;
1541                 MVert *mvert= pbvh->verts;
1542
1543                 vertCos= MEM_callocN(3*pbvh->totvert*sizeof(float), "BLI_pbvh_get_vertCoords");
1544                 co= (float*)vertCos;
1545
1546                 for (a= 0; a<pbvh->totvert; a++, mvert++, co+= 3) {
1547                         copy_v3_v3(co, mvert->co);
1548                 }
1549         }
1550
1551         return vertCos;
1552 }
1553
1554 void BLI_pbvh_apply_vertCos(PBVH *pbvh, float (*vertCos)[3])
1555 {
1556         int a;
1557
1558         if (!pbvh->deformed) {
1559                 if (pbvh->verts) {
1560                         /* if pbvh is not already deformed, verts/faces points to the */
1561                         /* original data and applying new coords to this arrays would lead to */
1562                         /* unneeded deformation -- duplicate verts/faces to avoid this */
1563
1564                         pbvh->verts= MEM_dupallocN(pbvh->verts);
1565                         pbvh->faces= MEM_dupallocN(pbvh->faces);
1566
1567                         pbvh->deformed= 1;
1568                 }
1569         }
1570
1571         if (pbvh->verts) {
1572                 MVert *mvert= pbvh->verts;
1573                 /* copy new verts coords */
1574                 for (a= 0; a < pbvh->totvert; ++a, ++mvert) {
1575                         copy_v3_v3(mvert->co, vertCos[a]);
1576                         mvert->flag |= ME_VERT_PBVH_UPDATE;
1577                 }
1578
1579                 /* coordinates are new -- normals should also be updated */
1580                 mesh_calc_normals(pbvh->verts, pbvh->totvert, pbvh->faces, pbvh->totprim, NULL);
1581
1582                 for (a= 0; a < pbvh->totnode; ++a)
1583                         BLI_pbvh_node_mark_update(&pbvh->nodes[a]);
1584
1585                 BLI_pbvh_update(pbvh, PBVH_UpdateBB, NULL);
1586                 BLI_pbvh_update(pbvh, PBVH_UpdateOriginalBB, NULL);
1587
1588         }
1589 }
1590
1591 int BLI_pbvh_isDeformed(PBVH *pbvh)
1592 {
1593         return pbvh->deformed;
1594 }
1595 /* Proxies */
1596
1597 PBVHProxyNode* BLI_pbvh_node_add_proxy(PBVH* bvh, PBVHNode* node)
1598 {
1599         int index, totverts;
1600
1601         #pragma omp critical
1602         {
1603
1604                 index = node->proxy_count;
1605
1606                 node->proxy_count++;
1607
1608                 if (node->proxies)
1609                         node->proxies= MEM_reallocN(node->proxies, node->proxy_count*sizeof(PBVHProxyNode));
1610                 else
1611                         node->proxies= MEM_mallocN(sizeof(PBVHProxyNode), "PBVHNodeProxy");
1612
1613                 if (bvh->grids)
1614                         totverts = node->totprim*bvh->gridsize*bvh->gridsize;
1615                 else
1616                         totverts = node->uniq_verts;
1617
1618                 node->proxies[index].co= MEM_callocN(sizeof(float[3])*totverts, "PBVHNodeProxy.co");
1619         }
1620
1621         return node->proxies + index;
1622 }
1623
1624 void BLI_pbvh_node_free_proxies(PBVHNode* node)
1625 {
1626         #pragma omp critical
1627         {
1628                 int p;
1629
1630                 for (p= 0; p < node->proxy_count; p++) {
1631                         MEM_freeN(node->proxies[p].co);
1632                         node->proxies[p].co= 0;
1633                 }
1634
1635                 MEM_freeN(node->proxies);
1636                 node->proxies = 0;
1637
1638                 node->proxy_count= 0;
1639         }
1640 }
1641
1642 void BLI_pbvh_gather_proxies(PBVH* pbvh, PBVHNode*** r_array,  int* r_tot)
1643 {
1644         PBVHNode **array= NULL, **newarray, *node;
1645         int tot= 0, space= 0;
1646         int n;
1647
1648         for (n= 0; n < pbvh->totnode; n++) {
1649                 node = pbvh->nodes + n;
1650
1651                 if(node->proxy_count > 0) {
1652                         if(tot == space) {
1653                                 /* resize array if needed */
1654                                 space= (tot == 0)? 32: space*2;
1655                                 newarray= MEM_callocN(sizeof(PBVHNode)*space, "BLI_pbvh_gather_proxies");
1656
1657                                 if (array) {
1658                                         memcpy(newarray, array, sizeof(PBVHNode)*tot);
1659                                         MEM_freeN(array);
1660                                 }
1661
1662                                 array= newarray;
1663                         }
1664
1665                         array[tot]= node;
1666                         tot++;
1667                 }
1668         }
1669
1670         if(tot == 0 && array) {
1671                 MEM_freeN(array);
1672                 array= NULL;
1673         }
1674
1675         *r_array= array;
1676         *r_tot= tot;
1677 }