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