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