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