svn merge -r37600:37700 https://svn.blender.org/svnroot/bf-blender/trunk/blender
[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                 if(totvert) *totvert= node->totprim*bvh->gridsize*bvh->gridsize;
1193                 if(uniquevert) *uniquevert= *totvert;
1194         }
1195         else {
1196                 if(totvert) *totvert= node->uniq_verts + node->face_verts;
1197                 if(uniquevert) *uniquevert= node->uniq_verts;
1198         }
1199 }
1200
1201 void BLI_pbvh_node_get_grids(PBVH *bvh, PBVHNode *node, int **grid_indices, int *totgrid, int *maxgrid, int *gridsize, DMGridData ***griddata, DMGridAdjacency **gridadj)
1202 {
1203         if(bvh->grids) {
1204                 if(grid_indices) *grid_indices= node->prim_indices;
1205                 if(totgrid) *totgrid= node->totprim;
1206                 if(maxgrid) *maxgrid= bvh->totgrid;
1207                 if(gridsize) *gridsize= bvh->gridsize;
1208                 if(griddata) *griddata= bvh->grids;
1209                 if(gridadj) *gridadj= bvh->gridadj;
1210         }
1211         else {
1212                 if(grid_indices) *grid_indices= NULL;
1213                 if(totgrid) *totgrid= 0;
1214                 if(maxgrid) *maxgrid= 0;
1215                 if(gridsize) *gridsize= 0;
1216                 if(griddata) *griddata= NULL;
1217                 if(gridadj) *gridadj= NULL;
1218         }
1219 }
1220
1221 void BLI_pbvh_node_get_BB(PBVHNode *node, float bb_min[3], float bb_max[3])
1222 {
1223         copy_v3_v3(bb_min, node->vb.bmin);
1224         copy_v3_v3(bb_max, node->vb.bmax);
1225 }
1226
1227 void BLI_pbvh_node_get_original_BB(PBVHNode *node, float bb_min[3], float bb_max[3])
1228 {
1229         copy_v3_v3(bb_min, node->orig_vb.bmin);
1230         copy_v3_v3(bb_max, node->orig_vb.bmax);
1231 }
1232
1233 void BLI_pbvh_node_get_proxies(PBVHNode* node, PBVHProxyNode** proxies, int* proxy_count)
1234 {
1235         if (node->proxy_count > 0) {
1236                 if (proxies) *proxies = node->proxies;
1237                 if (proxy_count) *proxy_count = node->proxy_count;
1238         }
1239         else {
1240                 if (proxies) *proxies = 0;
1241                 if (proxy_count) *proxy_count = 0;
1242         }
1243 }
1244
1245 /********************************* Raycast ***********************************/
1246
1247 typedef struct {
1248         /* Ray */
1249         float start[3];
1250         int sign[3];
1251         float inv_dir[3];
1252         int original;
1253 } RaycastData;
1254
1255 /* Adapted from here: http://www.gamedev.net/community/forums/topic.asp?topic_id=459973 */
1256 static int ray_aabb_intersect(PBVHNode *node, void *data_v)
1257 {
1258         RaycastData *ray = data_v;
1259         float bbox[2][3];
1260         float tmin, tmax, tymin, tymax, tzmin, tzmax;
1261
1262         if(ray->original)
1263                 BLI_pbvh_node_get_original_BB(node, bbox[0], bbox[1]);
1264         else
1265                 BLI_pbvh_node_get_BB(node, bbox[0], bbox[1]);
1266
1267         tmin = (bbox[ray->sign[0]][0] - ray->start[0]) * ray->inv_dir[0];
1268         tmax = (bbox[1-ray->sign[0]][0] - ray->start[0]) * ray->inv_dir[0];
1269
1270         tymin = (bbox[ray->sign[1]][1] - ray->start[1]) * ray->inv_dir[1];
1271         tymax = (bbox[1-ray->sign[1]][1] - ray->start[1]) * ray->inv_dir[1];
1272
1273         if((tmin > tymax) || (tymin > tmax))
1274                 return 0;
1275
1276         if(tymin > tmin)
1277                 tmin = tymin;
1278
1279         if(tymax < tmax)
1280                 tmax = tymax;
1281
1282         tzmin = (bbox[ray->sign[2]][2] - ray->start[2]) * ray->inv_dir[2];
1283         tzmax = (bbox[1-ray->sign[2]][2] - ray->start[2]) * ray->inv_dir[2];
1284
1285         if((tmin > tzmax) || (tzmin > tmax))
1286                 return 0;
1287
1288         if(tzmin > tmin)
1289                 tmin = tzmin;
1290
1291         // XXX jwilkins: tmax does not need to be updated since we don't use it
1292         // keeping this here for future reference
1293         //if(tzmax < tmax) tmax = tzmax; 
1294
1295         node->tmin = tmin;
1296
1297         return 1;
1298 }
1299
1300 void BLI_pbvh_raycast(PBVH *bvh, BLI_pbvh_HitOccludedCallback cb, void *data,
1301                           float ray_start[3], float ray_normal[3], int original)
1302 {
1303         RaycastData rcd;
1304
1305         copy_v3_v3(rcd.start, ray_start);
1306         rcd.inv_dir[0] = 1.0f / ray_normal[0];
1307         rcd.inv_dir[1] = 1.0f / ray_normal[1];
1308         rcd.inv_dir[2] = 1.0f / ray_normal[2];
1309         rcd.sign[0] = rcd.inv_dir[0] < 0;
1310         rcd.sign[1] = rcd.inv_dir[1] < 0;
1311         rcd.sign[2] = rcd.inv_dir[2] < 0;
1312         rcd.original = original;
1313
1314         BLI_pbvh_search_callback_occluded(bvh, ray_aabb_intersect, &rcd, cb, data);
1315 }
1316
1317 static int ray_face_intersection(float ray_start[3], float ray_normal[3],
1318                                  float *t0, float *t1, float *t2, float *t3,
1319                                  float *fdist)
1320 {
1321         float dist;
1322
1323         if ((isect_ray_tri_epsilon_v3(ray_start, ray_normal, t0, t1, t2, &dist, NULL, 0.1f) && dist < *fdist) ||
1324            (t3 && isect_ray_tri_epsilon_v3(ray_start, ray_normal, t0, t2, t3, &dist, NULL, 0.1f) && dist < *fdist))
1325         {
1326                 *fdist = dist;
1327                 return 1;
1328         }
1329         else {
1330                 return 0;
1331         }
1332 }
1333
1334 int BLI_pbvh_node_raycast(PBVH *bvh, PBVHNode *node, float (*origco)[3],
1335         float ray_start[3], float ray_normal[3], float *dist)
1336 {
1337         int hit= 0;
1338
1339         if(bvh->faces) {
1340                 MVert *vert = bvh->verts;
1341                 int *faces= node->prim_indices;
1342                 int *face_verts= node->face_vert_indices;
1343                 int totface= node->totprim;
1344                 int i;
1345
1346                 for(i = 0; i < totface; ++i) {
1347                         MFace *f = bvh->faces + faces[i];
1348
1349                         if(origco) {
1350                                 /* intersect with backuped original coordinates */
1351                                 hit |= ray_face_intersection(ray_start, ray_normal,
1352                                                          origco[face_verts[i*4+0]],
1353                                                          origco[face_verts[i*4+1]],
1354                                                          origco[face_verts[i*4+2]],
1355                                                          f->v4? origco[face_verts[i*4+3]]: NULL,
1356                                                          dist);
1357                         }
1358                         else {
1359                                 /* intersect with current coordinates */
1360                                 hit |= ray_face_intersection(ray_start, ray_normal,
1361                                                          vert[f->v1].co,
1362                                                          vert[f->v2].co,
1363                                                          vert[f->v3].co,
1364                                                          f->v4 ? vert[f->v4].co : NULL,
1365                                                          dist);
1366                         }
1367                 }
1368         }
1369         else {
1370                 int totgrid= node->totprim;
1371                 int gridsize= bvh->gridsize;
1372                 int i, x, y;
1373
1374                 for(i = 0; i < totgrid; ++i) {
1375                         DMGridData *grid= bvh->grids[node->prim_indices[i]];
1376                         if (!grid)
1377                                 continue;
1378
1379                         for(y = 0; y < gridsize-1; ++y) {
1380                                 for(x = 0; x < gridsize-1; ++x) {
1381                                         if(origco) {
1382                                                 hit |= ray_face_intersection(ray_start, ray_normal,
1383                                                                          origco[y*gridsize + x],
1384                                                                          origco[y*gridsize + x+1],
1385                                                                          origco[(y+1)*gridsize + x+1],
1386                                                                          origco[(y+1)*gridsize + x],
1387                                                                          dist);
1388                                         }
1389                                         else {
1390                                                 hit |= ray_face_intersection(ray_start, ray_normal,
1391                                                                          grid[y*gridsize + x].co,
1392                                                                          grid[y*gridsize + x+1].co,
1393                                                                          grid[(y+1)*gridsize + x+1].co,
1394                                                                          grid[(y+1)*gridsize + x].co,
1395                                                                          dist);
1396                                         }
1397                                 }
1398                         }
1399
1400                         if(origco)
1401                                 origco += gridsize*gridsize;
1402                 }
1403         }
1404
1405         return hit;
1406 }
1407
1408 //#include <GL/glew.h>
1409
1410 void BLI_pbvh_node_draw(PBVHNode *node, void *UNUSED(data))
1411 {
1412 #if 0
1413         /* XXX: Just some quick code to show leaf nodes in different colors */
1414         float col[3]; int i;
1415
1416         if(0) { //is_partial) {
1417                 col[0] = (rand() / (float)RAND_MAX); col[1] = col[2] = 0.6;
1418         }
1419         else {
1420                 srand((long long)node);
1421                 for(i = 0; i < 3; ++i)
1422                         col[i] = (rand() / (float)RAND_MAX) * 0.3 + 0.7;
1423         }
1424         glMaterialfv(GL_FRONT_AND_BACK, GL_DIFFUSE, col);
1425
1426         glColor3f(1, 0, 0);
1427 #endif
1428         GPU_draw_buffers(node->draw_buffers);
1429 }
1430
1431 /* Adapted from:
1432    http://www.gamedev.net/community/forums/topic.asp?topic_id=512123
1433    Returns true if the AABB is at least partially within the frustum
1434    (ok, not a real frustum), false otherwise.
1435 */
1436 int BLI_pbvh_node_planes_contain_AABB(PBVHNode *node, void *data)
1437 {
1438         float (*planes)[4] = data;
1439         int i, axis;
1440         float vmin[3] /*, vmax[3]*/, bb_min[3], bb_max[3];
1441
1442         BLI_pbvh_node_get_BB(node, bb_min, bb_max);
1443
1444         for(i = 0; i < 4; ++i) { 
1445                 for(axis = 0; axis < 3; ++axis) {
1446                         if(planes[i][axis] > 0) { 
1447                                 vmin[axis] = bb_min[axis];
1448                                 /*vmax[axis] = bb_max[axis];*/ /*UNUSED*/
1449                         }
1450                         else {
1451                                 vmin[axis] = bb_max[axis];
1452                                 /*vmax[axis] = bb_min[axis];*/ /*UNUSED*/
1453                         }
1454                 }
1455                 
1456                 if(dot_v3v3(planes[i], vmin) + planes[i][3] > 0)
1457                         return 0;
1458         } 
1459
1460         return 1;
1461 }
1462
1463 void BLI_pbvh_draw(PBVH *bvh, float (*planes)[4], float (*face_nors)[3], int smooth)
1464 {
1465         PBVHNode **nodes;
1466         int totnode;
1467
1468         BLI_pbvh_search_gather(bvh, update_search_cb, SET_INT_IN_POINTER(PBVH_UpdateNormals|PBVH_UpdateDrawBuffers),
1469                 &nodes, &totnode);
1470
1471         pbvh_update_normals(bvh, nodes, totnode, face_nors);
1472         pbvh_update_draw_buffers(bvh, nodes, totnode, smooth);
1473
1474         if(nodes) MEM_freeN(nodes);
1475
1476         if(planes) {
1477                 BLI_pbvh_search_callback(bvh, BLI_pbvh_node_planes_contain_AABB,
1478                                 planes, BLI_pbvh_node_draw, NULL);
1479         }
1480         else {
1481                 BLI_pbvh_search_callback(bvh, NULL, NULL, BLI_pbvh_node_draw, NULL);
1482         }
1483 }
1484
1485 void BLI_pbvh_grids_update(PBVH *bvh, DMGridData **grids, DMGridAdjacency *gridadj, void **gridfaces)
1486 {
1487         bvh->grids= grids;
1488         bvh->gridadj= gridadj;
1489         bvh->gridfaces= gridfaces;
1490 }
1491
1492 float (*BLI_pbvh_get_vertCos(PBVH *pbvh))[3]
1493 {
1494         int a;
1495         float (*vertCos)[3]= NULL;
1496
1497         if (pbvh->verts) {
1498                 float *co;
1499                 MVert *mvert= pbvh->verts;
1500
1501                 vertCos= MEM_callocN(3*pbvh->totvert*sizeof(float), "BLI_pbvh_get_vertCoords");
1502                 co= (float*)vertCos;
1503
1504                 for (a= 0; a<pbvh->totvert; a++, mvert++, co+= 3) {
1505                         copy_v3_v3(co, mvert->co);
1506                 }
1507         }
1508
1509         return vertCos;
1510 }
1511
1512 void BLI_pbvh_apply_vertCos(PBVH *pbvh, float (*vertCos)[3])
1513 {
1514         int a;
1515
1516         if (!pbvh->deformed) {
1517                 if (pbvh->verts) {
1518                         /* if pbvh is not already deformed, verts/faces points to the */
1519                         /* original data and applying new coords to this arrays would lead to */
1520                         /* unneeded deformation -- duplicate verts/faces to avoid this */
1521
1522                         pbvh->verts= MEM_dupallocN(pbvh->verts);
1523                         pbvh->faces= MEM_dupallocN(pbvh->faces);
1524
1525                         pbvh->deformed= 1;
1526                 }
1527         }
1528
1529         if (pbvh->verts) {
1530                 MVert *mvert= pbvh->verts;
1531                 /* copy new verts coords */
1532                 for (a= 0; a < pbvh->totvert; ++a, ++mvert) {
1533                         copy_v3_v3(mvert->co, vertCos[a]);
1534                         mvert->flag |= ME_VERT_PBVH_UPDATE;
1535                 }
1536
1537                 /* coordinates are new -- normals should also be updated */
1538                 mesh_calc_tessface_normals(pbvh->verts, pbvh->totvert, pbvh->faces, pbvh->totprim, NULL);
1539
1540                 for (a= 0; a < pbvh->totnode; ++a)
1541                         BLI_pbvh_node_mark_update(&pbvh->nodes[a]);
1542
1543                 BLI_pbvh_update(pbvh, PBVH_UpdateBB, NULL);
1544                 BLI_pbvh_update(pbvh, PBVH_UpdateOriginalBB, NULL);
1545
1546         }
1547 }
1548
1549 int BLI_pbvh_isDeformed(PBVH *pbvh)
1550 {
1551         return pbvh->deformed;
1552 }
1553 /* Proxies */
1554
1555 PBVHProxyNode* BLI_pbvh_node_add_proxy(PBVH* bvh, PBVHNode* node)
1556 {
1557         int index, totverts;
1558
1559         #pragma omp critical
1560         {
1561
1562                 index = node->proxy_count;
1563
1564                 node->proxy_count++;
1565
1566                 if (node->proxies)
1567                         node->proxies= MEM_reallocN(node->proxies, node->proxy_count*sizeof(PBVHProxyNode));
1568                 else
1569                         node->proxies= MEM_mallocN(sizeof(PBVHProxyNode), "PBVHNodeProxy");
1570
1571                 if (bvh->grids)
1572                         totverts = node->totprim*bvh->gridsize*bvh->gridsize;
1573                 else
1574                         totverts = node->uniq_verts;
1575
1576                 node->proxies[index].co= MEM_callocN(sizeof(float[3])*totverts, "PBVHNodeProxy.co");
1577         }
1578
1579         return node->proxies + index;
1580 }
1581
1582 void BLI_pbvh_node_free_proxies(PBVHNode* node)
1583 {
1584         #pragma omp critical
1585         {
1586                 int p;
1587
1588                 for (p= 0; p < node->proxy_count; p++) {
1589                         MEM_freeN(node->proxies[p].co);
1590                         node->proxies[p].co= 0;
1591                 }
1592
1593                 MEM_freeN(node->proxies);
1594                 node->proxies = 0;
1595
1596                 node->proxy_count= 0;
1597         }
1598 }
1599
1600 void BLI_pbvh_gather_proxies(PBVH* pbvh, PBVHNode*** r_array,  int* r_tot)
1601 {
1602         PBVHNode **array= NULL, **newarray, *node;
1603         int tot= 0, space= 0;
1604         int n;
1605
1606         for (n= 0; n < pbvh->totnode; n++) {
1607                 node = pbvh->nodes + n;
1608
1609                 if(node->proxy_count > 0) {
1610                         if(tot == space) {
1611                                 /* resize array if needed */
1612                                 space= (tot == 0)? 32: space*2;
1613                                 newarray= MEM_callocN(sizeof(PBVHNode)*space, "BLI_pbvh_gather_proxies");
1614
1615                                 if (array) {
1616                                         memcpy(newarray, array, sizeof(PBVHNode)*tot);
1617                                         MEM_freeN(array);
1618                                 }
1619
1620                                 array= newarray;
1621                         }
1622
1623                         array[tot]= node;
1624                         tot++;
1625                 }
1626         }
1627
1628         if(tot == 0 && array) {
1629                 MEM_freeN(array);
1630                 array= NULL;
1631         }
1632
1633         *r_array= array;
1634         *r_tot= tot;
1635 }