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